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Abstract  of  Dissertation  Presented  to  the  Graduate  School 
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This  dissertation  develops  new  techniques  for  robot  motion  planning 
and  force  coordination  in  multi-manipulator  tasks. 

We  develop  a  new  model  for  joint  interpolated  motion.  A  constraint 
on  the  trajectories  of  the  faster,  non-pacing  joints  clips  both  veloci- 
ties and  accelerations  of  the  non-pacing  joints  without  affecting  the 
total  move  time.  Experimental  and  simulation  tests  on  the  joint  motion 
model  clearly  show  the  improved  performance  in  controlled  and  smooth 
start-ups  and  set-downs  of  the  manipulator  joints  and  the  end  effector. 

This  dissertation  develops  the  first  complete  formulation  for  on- 
line tracking  of  an  end  effector  trajectory  by  piece-wise  cubic  spline 
joint  trajectories.  A  factor  called  the  system  memory  error  is  defined 
to  estimate  an  ideal  number  of  look-ahead  points  on  the  end  effector 
trajectory  for  a  prescribed  level  of  performance  in  the  joint 
trajectories.  Two  methods  are  described  for  estimation  of  an  additional 
constraint  at  the  end  points  of  each  computational  block  of  the  on-line 
joint    trajectory    generator.       Excellent    results    were    obtained  in 
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simulation  and  experimental  tests  of  the  on-line  splines  for  accuracy  in 
tracking  and  smoothness  of  motion  for  a  large  number  of  constrained  end 
effector  motions. 

A  new  dimension  to  robot  motion  planning  is  added  by  the  intro- 
duction, definition  and  development  of  independent  rotational  motion 
planning  for  constrained  and  unconstrained  rotations  of  the  robot  end 
effector.  We  define  a  rotation  space  at  the  wrist  joint  of  spherical 
wrist  manipulators  and  prove  that  the  maximum  conf i guraton-f ree  tool 
rotational  acceleration  in  the  decoupled  rotation  space  is  the  full-load 
acceleration  of  the  slowest  joint  in  rotation  space.  A  simulation 
example  with  independent  rotational  planning  illustrates  the  elegance  of 
this  approach  for  planning  "feasible"  and  accurate  rotational  motions. 

We  develop  two  linear  models  for  "quasi -static"  load  distribution 
in  the  coordination  of  a  closed-chain  multi-manipulator  task.  The 
models  employ  moving  coordinate  frames  on  the  object  as  reference  frames 
for  load  distribution.  The  moving  frames,  called  the  force  frames,  are 
defined  from  the  trajectory  space  curves  of  the  object  and  the  end 
effectors.  We  show  that  the  constraint  specification  of  the  load 
distribution  problem  is  simplified  by  the  computation  of  all  constraints 
in  terms  of  the  force  frames.  The  load  distribution  is  set  up  as  a 
linear  programming  problem  with  constraints  on  the  maximum  surface 
normal  forces  and  the  maximum  joint  torques.  The  objective  function 
minimizes  the  system  power  and  attempts  a  load  distribution  in  the  ratio 
of  the  individual  manipulator  payloads. 
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CHAPTER  I 
INTRODUCTION 


The    recent    years    have    seen    an    explosive    growth    in  factory 
automation.       Increasing    demands    on    productivity,    work    in  hostile 
environments    and    mundane    and    repetitive    tasks    call    for  immediate 
application    of    robotic    technology.       As    demands    on    the  robot's 
performance  increase,  the  level  of  sophistication  of  robotic  technology 
must    also    increase.      The    articulated    robot    manipulator,    a  design 
motivated   by   the   dexterous   human   arm,    is   commonly   applied   in  many 
industries  for  tasks  ranging  from  assembly  and  part  sorting  to  welding 
and  heavy  material    handling.      The   productivity   in   a  task  involving 
several  sub-tasks  can  be  considerably  increased  if  multiple  manipulators 
work  cooperatively  on  the  sub-tasks.     It  is  easy  to  conceive  of  a  large 
number    of   tasks   that    can    be   made    faster   and   more   efficient  with 
coordinated  movements  of  multiple  robots  working  cooperatively.    A  few 
multi-robot      systems      available     commerically     provide  restricted 
interaction  between  the  multiple  robots  in  the  system.    Olivetti  was  the 
first  to  market  an  industrial   robot  system  with  multi-arm  option  as  a 
special    feature    [Bedina    1977].      The   Olivetti    SIGMA   system   can  be 
configured  with  two  or  three  arms,  the  only  restriction  being  that  the 
total   number  of  controlled  degrees  of  freedom  (motors)  cannot  exceed 
eight.    A  limited  amount  of  cooperation  is  allowed  between  the  robots. 
Although  the  arms  work  in  the  same  workspace,  the  sub-tasks  for  each  arm 
are  independent.    Collisions  are  avoided  by  stopping  one  arm  until  the 
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other  moves  out  of  the  way.  Thus,  the  Olivetti  system  is  meant  for 
tasks  with  independent  sub-tasks,  since  direct  interaction  between  the 
multiple  robots  is  not  allowed.  In  Japan,  the  Japanese  government 
research  institute  has  developed  a  prototype  for  an  industrial  robot 
called  "Melarm"  [Nakano  et  al .  1974].  Melarm  has  two  arms,  each  with  7 
joints  and  a  gripper.  Both  arms  are  equipped  with  force  feedback  on 
every  joint  and  several  tactile  sensors.  An  experimental  system  for 
automatic  assembly  of  vacuum  cleaners  using  two  8  degrees  of  freedom 
robots  has  been  developed  by  Hitachi  Ltd.  [Takeyasu  1977].  Each  arm  has 
3  fingers  and  about  30  tactile  sensors  on  its  grippers.  There  is  no 
doubt  that  industry  is  aware  of  the  numerous  applications  of  cooperating 
robots  to  perform  tasks  hitherto  impossible  (or  inefficient)  for  a 
single  robot  working  alone. 

Research  in  robotics  is  distributed  in  diverse  interdisciplinary 
areas  such  as  sensory  preception  (vision,  tactile  and  force)  [Gleason 
and  Agin  1979,  Inoue  1974,  Raibert  and  Tanner  1982,  Stute  and  Erne 
1979],  manipulator  design  [Warnecke  and  Shraft  1979],  motion  planning 
[Paul  1979,  Lozano-Perez  1981,  Whitney  1972],  transformations  [Duffy 
1981,  Paul  1981],  dynamics  and  servo  control  [Bejczy  1974,  Golla  et  al . 
1981,  Hollerbach  1979,  Luh  et  al .  1980b]  and  locomotion  [Giralt  et  al . 
1979,  McGhee  and  Orin  1976,  Taguchi  et  al .  1976].  This  dissertation 
attempts  to  provide  the  sophistication  in  the  techniques  for  robot 
motion  planning  and  force  coordination  needed  to  provide  multiple  robots 
the  capability  to  perform  tasks  requiring  a  high  degree  of  interaction 
between  the  robots.  Most  of  the  techniques  developed  in  this 
dissertation  are  directly  applicable  to  a  single  robot  controller  for 
better  path  tracking  in  end  effector  controlled  motions,  smoother  start- 
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up  and  set-down  in  joint  interpolated  motion  and  planning  independent 
rotational  motions  to  make  a  rotation  dominant  trajectory  "feasible" 
with  smaller  rotational  errors. 

In  Chapter  II,  we  reformulate  the  trajectory  generation  problem, 
both  for  joint  interpolated  motion  and  end  effector  controlled  motion. 
The  reformulation  is  either  an  enhancement  of  the  existing  techniques  or 
a  new  solution  to  the  problem.  At  the  joint  level,  we  formulate  the 
joint  interpolated  motion  by  a  model  that  allows  the  start-up  and  set- 
down  accelerations  of  each  joint  to  be  controlled.  We  introduce  a  new 
constraint  on  the  motion  of  the  non-pacing  joints  that  clips  the  start- 
up and  set-down  acceleration  as  well  as  the  velocity  of  the  non-pacing 
joints,  without  affecting  the  total  move  time.  The  performance  of 
the  proposed  model  was  tested  on  a  6-degrees  of  freedom  industrial 
robot.  The  results  clearly  show  the  superiority  of  the  proposed 
model  to  the  existing  model  [Anderson  and  Paul  1979,  Paul  1981,  Whitney 
1972]. 

A  new  solution  is  derived  for  the  on-line  joint  trajectory 
approximation  [Paul  1979]  during  a  constrained  end  effector  motion  in 
Cartesian  space.  The  on-line  joint  trajectories  are  constructed  from 
the  current  point  and  a  few  look-ahead  points  on  the  end  effector 
trajectory.  Most  robot  controllers  employ  a  2-point  look-ahead  on  the 
end  effector  trajectory  in  order  to  linearly  interpolate  the  joint 
coordinates  between  the  current  point  and  the  two  look-ahead  points,  and 
provide  a  quadratic  smoothing  from  one  segment  to  another  if  a 
discontinuity  in  velocity  is  detected  between  the  two  linear  segments. 
A  linear-quadratic  interpolation  of  the  joint  trajectory  with  a  2-point 
look-ahead  [Paul  1979]  produces  considerable  tracking  error  with  higher 
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velocities  and  lower  sample  rates  on  the  end  effector  trajectory.  In 
order  to  produce  a  better  approximation  of  the  joint  trajectories  with  a 
limited  point  look-ahead,  we  develop  a  scheme  for  on-line  approximation 
of  the  joint  trajectories  by  piecewise  cubic  splines.  We  formalize  the 
estimation  of  the  ideal  number  of  look-ahead  points  on  the  end  effector 
trajectory  in  order  to  produce  an  accurate  approximation  of  the  joint 
trajectories  by  piece-wise  cubic  splines.  We  define  a  factor  called  the 
system  memory  error,  as  a  measure  of  the  coupling  errors  in  the 
estimation  of  the  joint  velocities  in  successive  computational  blocks  on 
the  on-line  cubic  splines  trajectory.  The  ideal  number  of  look-ahead 
points  is  defined  such  that  the  coupling  in  the  determination  of  the 
boudary  conditions  of  successive  computational  blocks  is  lower  than  a 
specified  threshold.  The  derivation  shows  that  for  a  piece-wise  cubic 
splines  approximation  of  the  joint  trajectories  with  equidistant  knots, 
4  look-ahead  points  in  each  block  produces  a  system  memory  error  of  less 
than  0.5  percent.  This  result  indicates  that  for  an  equidistant  cubic 
splines  approximation,  a  reasonably  good  trajectory  approximation  with 
minimal  coupling  between  successive  computational  blocks  (less  than  0.5 
percent)  can  be  obtained  with  4  look-ahead  points  on  the  end  effector 
trajectory. 

A  derivation  for  the  acceleration  jumps  on  the  on-line  cubic 
splines  for  the  joint  trajectory  approximation  shows  that  the 
acceleration  jump,  to  a  first  approximation,  reduces,  when  successive 
computational  blocks  are  overlapped  for  improving  the  velocity  estimate 
at  the  block  end  points.  The  on-line  cubic  splines  improve  the  tracking 
accuracy  of  the  end  effector  by  providing  a  better  approximation  of  the 
joint     trajectories.        The     improved    tracking     is     illustrated  by 
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sumulation.  The  on-line  cubic  splines  were  implemented  on  an  industrial 
robot  controller  and  tested  for  a  large  number  of  constrained  end 
effector  moves  with  different  velocities  and  sample  update  rates.  The 
on-line  spline  approximation  technique  showed  excellent  results  in  terms 
of  smooth  motion  and  tracking  accuracy,  for  sample  rates  as  high  as 
200ms  and  end  effector  velocities  of  up  to  1.5  m/sec. 

The  current   techniques   for   end   effector   motion   planning   for  a 
single  robot  are  "translation  dominant"  [Finkel  1976,  Paul  1979,  Taylor 
1979].    In  other  words,  the  planning  of  an  end  effector  motion  is  based 
on  the  constraints  on  the  end  effector  linear  velocity  and  accelera- 
tion.   Thus,  for  a  given  move,  the  constraints  on  the  tool  translational 
motion  are  employed  to  determine  the  move  time.    This  move  time  is  also 
forced     on     the     rotational     trajectory.         Independent  rotational 
trajectories  have  been  underplayed  in  robot  motion  since  rotations  are 
difficult  to  visualize,  and  the  determination  of  the  maximum  constraints 
on  the  tool  rotational  motion  is  also  difficult.    Therefore,  as  long  as 
the  rotational  tool  motion  is  uniform,  no  constraints  are  imposed  on  the 
rotational  trajectory.     Since  translation  and  rotation  are  independent 
motions,  rotational  planning  should  be  decoupled  from  the  translational 
move  planning.    In  Chapter  III,  we  introduce  and  develop  the  concept  of 
independent  rotation  planning  in  the  robot   "rotation  space."      It  is 
shown  that  for  a  translation-dominant  planning  of  a  common  move  where 
the  robot  end  effector  transits  from  one  linear  move  segment  to  another, 
the    rotational     error,     as    defined    in    Chapter    III,    is  directly 
proportional   to  the  translation  move  time,  an  unjustified  dependence 
that  is  not  necessary.    Also,  for  several  robot  moves,  the  translation 
dominant  approach  may  not  yield  a  rotational  trajectory  that  conforms  to 
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the  maximum  constraints  of  the  manipulator  joint  velocities  and 
accelerations. 

We    employ    a    common    geometry    of    industrial     robots,  namely, 
manipulators   with    spherical    wrists,    to   derive   a    simple   method  for 
independent  rotational  planning.    For  a  spherical  wrist  manipulator,  we 
decouple  translations  and  rotations  at  the  wrist  joint.     The  first  3 
degrees    of    freedom,    from   the    base    to    the    wrist,    are    called  the 
positioning  degrees  of  freedom.    The  last  3  degrees  of  freedom  are  said 
to  constitute  the  rotation  space  of  the  spherical  wrist  manipulator. 
This  definition  of  rotation  space  simplifies  the  independent  planning  of 
the  tool   rotations.     Since  the  manipulator  is  decoupled  at  the  wrist 
joint,  and  we  independently  determine  the  rotation  degrees  of  freedom 
and  the  translation  degrees  of  freedom,  the  reverse  solution  for  the 
manipulator  is  simplified,  since  we  separately  determine  the  positional 
and  rotational  joints.    Thus,  only  a  pseudo  reverse  solution  is  required 
to  determine  the  positioning  degrees   of  freedom  when  the  rotational 
trajectory  is  pre-planned  in  rotation  space.    The  concept  of  a  pseudo- 
reverse    solution    with    pre-planned    rotational    motion    is  extremely 
powerful    for   producing   fast   and   accurate  end  effector  translational 
trajectories  for  the  class  of  .mani pul ators  with  non-spheri.cal  or  offset 
wrists,  that  only  permit  a  recursive  reverse  transformation. 

In  order  to  plan  rotational  motions  independently,  we  require  an 
estimate  of  the  maximum  rotational  acceleration  of  the  tool  at  any  point 
in  the  robot  workspace.  The  decoupling  of  the  translation  and  rotation 
ensures  a  less  conservative  estimate  of  the  rotational  acceleration  as 
compared  to  the  case  where  the  decoupling  is  not  possible,  since  the 
rotational    velocity   and   the   acceleration   constraints   are  determined 
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independently  for  the  first  3  and  the  last  3  degrees  of  freedom.  The 
last  3  joint  angles  are  usually  twice  a  fast  as  the  first  3  joint  angles 
and  therefore  yield  a  higher  estimate  of  the  tool  rotational  velocity 
and  acceleration.  We  derive  the  maximium  limit  on  a  value  of  the  tool 
rotational  acceleration  that  can  be  employed  any  where  in  the  usable 
robot  workspace  (except  at  the  singularity  and  limit  points)  and  show 
that  this  acceleration  is  the  full-load  acceleration  of  the  slowest 
joint  in  the  decoupled  rotation  space.  We  also  describe  an  algorithm 
for  unconstrained  point  to  point  rotational  transitions  employing  the 
maximum  limitations  on  the  joint  rates  and  accelerations  for  a 
"feasible"  rotational  trajectory.  A  simulation  of  independent 
rotational  motion  planning  clearly  illustrates  the  elegance  of  the 
approach  for  producing  smooth  and  feasible  rotational  motions  for  both 
constrained  and  unconstrained  tool  rotations. 

In  Chapters  II  and  III,  we  developed  new  solutions  for  joint 
interpolated  motion,  accurate  on-line  approximation  of  joint  space 
trajectories  and  the  construction  of  independent  rotational  motions  of 
the  robot  end  effector.  Coupled  with  the  existing  techniques  for 
translational  motion  planning,  we  now  have  the  capability  to  generate 
accurate  and  feasible  joint  trajectories  and  end  effector  motions  for  a 
given  robot  move.  Let  us  consider  the  task  of  displacing  a  long  or 
heavy  object  by  one  or  more  manipulators.  Given  the  destination  of  the 
object  and  the  desired  trajectory  of  the  object,  we  can  generate  the  end 
effector  and  joint  trajectories  of  each  robot.  In  order  to  determine 
the  torque  for  each  joint  motor  for  a  given  trajectory,  we  require  an 
active  distribution  of  the  object  load  among  the  manipulator  end 
effectors  holding  the  object  at  each  sample  point  on  the  trajectory. 
Dynamic  load  distribution  in  a  closed-loop  multi-manipulator  task  is 


8 

underspecif ied  [Orin  and  Oh  1981,  Williams  and  Seireg  1979],  Since 
theoretically  infinite  solutions  for  the  load  distribution  are  possible, 
in  Chapter  IV,  we  develop  two  linear  models  with  an  objective  function 
for  a  load  distribution  that  minimizes  the  overall  system  power. 

We  formulate  the  load  distribution  in  Chapter  IV  in  moving 
coordinate  frames  at  each  end  effector.  A  moving  coordinate  frame 
called  the  force  frame  is  defined  from  the  space  curve  of  each  end 
effector  trajectory.  The  3  axes  of  the  force  frame  are  the  tangent, 
normal,  and  bi-normal  of  the  end  effector  trajectory  space  curve.  The 
underspecif ied  load  distribution  is  set  us  as  a  linear  programming 
problem,  with  an  objective  function  for  minimization  of  the  system  power 
for  unit  forces  and  moments  in  each  direction  of  the  force  frame  axes. 
By  eliminating  a  direct  dependence  of  the  objective  function  on 
individual  joint  constraints,  the  objective  function  can  be  initially 
evaluated  as  any  nonlinear  function  of  the  joint  parameters  for  unit 
forces  and  moments  in  the  directions  of  the  force  frame  axes.  This  non- 
linearity  does  not  affect  the  linearity  of  our  model,  since  the 
objective  function  is  made  independent  of  the  individual  joint 
parameters.  In  order  to  verify  the  load  distribution  algorithm,  we 
simulate  a  simple  two  manipulator  closed-loop  task.  The  simulation 
illustrates  that  the  load  distribution  algorithm  of  Chapter  IV  reduces 
the  overall  joint  torques  of  the  manipulators  in  a  closed-loop  task.  An 
"equal"  load  distribution  is  employed  to  provide  a  comparison  of  the 
overall  joint  torques  for  the  load  distribution  scheme  of  Chapter  IV. 

Chapter  V  summarizes  the  main  ideas  in  this  dissertation,  and 
provides  insight  into  future  work  in  the  cooperation  and  coordination  of 
multi  robot  operation. 


CHAPTER  II 

ON-LINE  POLYNOMIAL  TRAJECTORIES  FOR  ROBOT  MANIPULATORS 

A  smooth,  accurate  and  predictable  trajectory  of  all  robot  joints 
is  of  utmost  importance  in  a  multi-robot  task.  Smooth  trajectories  for 
the  robot  joints  and  consequently,  the  end  effector,  are  a  necessity  for 
cooperating  robots.  Robot  motions  can  be  classified  into  two  types. 
One  type  of  motion  is  called  the  joint  interpolated  motion.  The  other 
type  is  controlled  motions  of  the  robot  end  effector.  In  a  joint 
interpolated  move,  the  robot  is  moved  from  one  set  of  joint  coordinates 
to  another  without  any  control  over  the  end  effector  trajectory.  We 
develop  a  model  that  employs  a  splined  polynomial  trajectory  for  the 
joint  trajectories  in  a  joint  interpolated  move  and  describe  a  technique 
for  scaling  both  the  velocity  and  acceleration  of  the  faster  joints  in  a 
joint  to  joint  move. 

Traditionally,  there  are  two  approaches  to  planning  the  joint 
trajectories  for  controlled  end  effector  motions.  One  is  the  off-line 
approach  where  all  the  joint  trajectories  are  planned  prior  to  the 
move.  The  other  approach  is  to  determine  the  joint  trajectories  on-line 
at  a  certain  sample  rate  during  the  actual  motion.  The  planning  of  off- 
line trajectories  is  generally  not  a  time  critical  process.  Therefore, 
several  well-k.iown  techniques  for  polynomial  trajectory  approximation 
are  directly  applicable,  the  complexity  and  the  accuracy  of 
approximation  is  determined  by  the  task  requirements.  The  on-line 
trajectory  problem,  however,  has   received  little  attention  since  the 
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lack  of  sufficient  number  of  constraints  on  the  on-line  trajectory  and 
the  time  constraint  for  computation  limits  the  degree  of  accuracy  of 
polynomial  approximation.  In  this  chapter,  we  develop  the  constraints 
on  the  on-line  trajectory  in  order  to  produce  a  cubic  splined  trajectory 
for  each  joint.  We  derive  the  ideal  number  of  look-ahead  points  for 
generating  an  on-line  joint  trajectory,  based  on  an  approximation  error 
factor  called  the  system  memory  error.  An  analysis  of  the  acceleration 
jumps  in  the  on-line  cubic  spline  trajectories  is  also  given.  The 
application  of  these  trajectories  to  robot  motion  has  been  studied  both 
by  simulation  and  actual  experimentation  on  an  industrial  robot.  Both 
studies  indicate  excellent  results  in  terms  of  smoothness  of  motion  and 
accuracy  of  tracking  the  actual  trajectory. 

Coordinated  Joint  Motion  —  An  Improved  Technique 

This  section  describes  joint  interpolated  motion  employing  a 
splined  polynomial  trajectory.  The  most  frequent  use  of  a  joint 
interpolated  move  in  an  industrial  environment  is  for  fast  transits  of 
the  robot  arm  between  jobs.  A  joint  interpolated  move  provides  the 
fastest  transition  of  the  robot  arm  in  its  workspace.  The  other  type  of 
move,  namely  a  controlled  motion  of  the  end  effector,  is  slower  due  to 
the  conservative  upper  limit  on  the  end  effector  speed  (a  value  that  is 
determined  from  the  joint  parameters  and  is  valid  for  the  entire  robot 
workspace).  Therefore,  a  joint  interpolated  motion  is  employed  for  fast 
but  unconstrained  transitions  oi  the  robot  end  effector  between  points. 

A  well  known  technique  for  joint  interpolated  motion  is  to 
determine  the  slowest  joint  for  the  move  followed  by  scaling  the 
velocities  of  the  remaining  joints  such  that  the  move  time  of  each  joint 
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is  that  of  the  slowest  joint  [Anderson  and  Paul  1979].  All  joints  are 
initially  accelerated  to  their  maximum  accelerations  until  the  desired 
joint  velocity  is  achieved.  The  joints  then  cruise  at  the  desired 
velocities  until  the  end  of  the  move,  where  the  maximum  deceleration  is 
applied  on  each  joint.  Due  to  the  fast  initial  start-up  of  all  the 
joints  at  their  maximum  accelerations,  the  end  effector  (whose 
trajectory  is  not  a  concern  in  joint  interpolated  motion)  experiences  a 
large  discontinuity  in  motion  at  the  start  (and  end)  of  the  motion.  If 
the  effector  has  a  delicate  tool  fixture,  the  user  may  be  prompted  to 
use  tool  controlled  motion  (and  sacrifice  speed)  for  intermediate 
transitions.  In  order  to  reduce  the  initial  discontinuity  in  end 
effector  motion,  we  model  a  joint  trajectory  such  that  we  start  each 
joint  motion  by  a  cubic  polynomial.  The  slope  of  the  acceleration  can 
be  defined  by  the  user  as  the  slowness  for  start-up.  After  a  slow 
start-up,  the  end  effector  motion  is  smooth,  since  all  the  joint 
trajectories  are  continuous.  We  define  a  general  model  for  joint 
interpolated  motions  employing  a  cubic  start-up  with  a  specified  degree 
of  "slowness."  If  the  speed  of  the  transition  is  of  higher  concern  than 
the  smoothness  in  start-up  and  set-down,  the  cubic  polynomial  can  be 
simplified  to  a  quadratic. 

The  new  idea  presented  in  this  section  is  to  coordinate  the  motion 
of  all  joints  during  start-up,  constant  velocity  and  set-down 
segments.  Thus,  instead  of  coordinating  the  joint  motion  between  start 
and  stop  points,  the  joints  are  required  to  be  on  the  start-up  segment, 
constant  velocity  or  the  set-down  segment  for  the  same  periods  of  time, 
determined  by  a  pacing  joint.  It  is  shown  that  by  assuming  the  same 
start-up  time  for  all  the  joints,  initial  jerk  on  some  of  the  non-pacing 
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joints  is  reduced,  making  the  start-up  (and  set-down)  motion  smoother. 
If  the  joint  motion  has  to  pass  through  an  intermediate  point  then  we 
could  either  use  a  cubic  spline  to  pass  through  the  point,  or  a 
quadratic  polynomial  as  in  [Paul  1979,  Taylor  1979]  for  by-passing  the 
point.  A  cubic  spline  is  likely  to  produce  an  overshoot  at  the 
intermediate  point,  which  may  be  unacceptable  if  the  joint  is  close  to  a 
limit  at  the  intermediate  value.  Therefore,  a  quadratic  flyby  is 
preferable  if  overshoot  is  not  desired.  Once  again,  all  joints  are  on 
the  flyby  path  for  the  same  period  of  time  determined  by  a  pacing  joint 
for  flyby.  By  forcing  all  joints  to  be  on  the  flyby  path  for  the  same 
period  of  time,  acceleration  requirement  of  the  non-pacing  joints  is 
reduced.  We  now  develop  a  model  for  the  joint  trajectories  for  a  point- 
to-point  joint  interpolated  motion. 

In  Figure  2.1,  let  J-^  and  J2  be  the  start  and  stop  points  for  a 
point-to-point  move  of  a  joint.  The  joint  trajectory  is  divided  into  3 
segments  called  the  start-up,  cruise,  and  set-down  segments.  A  cubic 
polynomial  is  employed  for  the  start-up  and  set-down  segments  in  order 
to  vary  the  speed  of  start-up  and  set-down.  Assuming  the  joint  starts 
from  rest,  the  joint  acceleration  increases  linearly  from  0  to  the 
required  acceleration  for  start-up.  The  acceleration  profile  of  the 
joint  is  made  up  of  3  segments.  For  an  initial  period  t^^.^.,  the  joint 
accelerates  linearly  to  the  maximum  servo  acceleration.  The  maximum 
acceleration  remains  constant  for  a  period  t^Q^^g^  and  then  drops 
linearly  to  zero  in  another  t^^^  seconds,  such  that  the  desired  velocity 
of  the  joint  is  achieved.  The  joint  acceleration  for  the  start-up  time 
tg  can  be  modeled  as  follows. 
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Figure  2.1 


A  sample  joint  trajectory 
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0  <  t  <  t 

-  acc 


a(t)  =a  t<t<(t+t^) 
'        max  acc  -     -  ^  acc  const' 


W  -  ^      -  ^acc  -  ^const)      (^acc  '  ^const^  <-  ^  <  ^a  (^'^^ 


where  a^g^  is  the  maximum  servo  acceleration    (known   from  the  servo 

characteristics)   and  m  is   a  user  defined  slope.     The  "slowness"  of 

start-up  and  set-down  is  determined  by  m.    The  polynomial  for  the  joint 

trajectory   will    be  a  cubic   for    0  <  t  <  t         and     (t      +  t  A 

-     -    acc  ^  acc  const' 

<  t  <  t  ,  and  quadratic  for  t^^^  <  t  <  t  .  If  t,-_  =  0,  then  the 
-     -    d  acc  -     -    a  act. 


start-up  and  set-down  polynomials  will   be  quadratics.     Also,  if  t 


a 


2tgj.j.,  the  polynomials  will  be  cubic.  The  condition  to  check,  namely 
>  2tg^^  ,  is  equivalent  to  v^  >  a^^^^/m  ,  where  v^  is  the  desired 
velocity  of  the  joint. 

In  order  to  produce  the  fastest  transition  during  the  cruise 
segment,  we  employ  a  polynomial  of  degree  one  for  the  joint  trajectory 
between  the  start-up  and  the  set-down  segments.  The  motions  of  all 
joints  are  coordinated  by  the  determination  of  the  joint  that  takes  the 
most  time  for  the  point-to-point  move.  This  joint  is  termed  the  pacing 
joint.  The  pacing  joint  time  scales  the  move  parameters  of  the  non- 
pacing  joints. 

Integrating  Eqn.  (2.1), 


Vq  +  0.5  mt 


0  <  t  <  t. 


acc 


v(t)  =    v(t     )  +  a     (t  -  t  ) 
^  acc'       max^  acc' 


Sec  -<  ^  -<  (^cc  "  Sonst) 
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v(t      +  t      ^)  +  a     (t  -  t       -  t  J 

acc      const'      max^        acc  const' 


0.5  m  (t  -  t       -  t  J' 
acc  const' 


(t      +  t       ^)  <  t  <  t 
^  acc      const'  -     -  a 


(2.2) 


where  Vq  is  the  initial  or  start  velocity,  v(tg^j.)  is  the  velocity  after 
tacc  seconds.  vCt^^^  +  t^^^^^)  is  the  velocity  after  (t^^c  +  t.^.^^) 
seconds.  Integrating  Eqn.  (2.2)  to  get  the  equation  for  the  joint 
displacement,  and  assuming  v^  =  0  (joint  starts  from  rest). 


0(t)  = 


J(0)  +  mt  /6 


0  <  t  <  t 


acc 


J(t,,,)  +  v(t^^^)  (t  -  t_)  +  0.5  a       (t  -  t  )' 
acc'         acc'  ^        acc'  max  ^  acc' 


t       <  t  <  (t      +  t  ^) 
acc  -     -  ^  acc  const' 


'acc  const' 


^max       -  ^acc  "  ^const^'/^  -  m  (t  -  t^^^  -  t^^^^^)3/6 


(^acc  '  ^const)  i  *  1  ^ 


(2.3) 


Let  denote  the  specified  velocity  of  a  joint  for  the  point-to- 
point  move.     This  velocity  is  usually  given  as  a  percentage  of  the 
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maximum  joint  velocity.  The  start-up  and  set-down  time  t^  can  be 
determined  from  Eqn.  (2.2)  as 


ta  = 


V  ./a  +  a  /m 
d    max  max 


(2v^/m) 


f/2 


t    >  2t 
a  acc 


t    =  2t 
a  acc 


(2.4) 


Let  tgj^  denote  the  time  for  the  cruise  segment.    If  Ae  is  the  joint 
displacement  between  the  2  points  of  the  point-to-point  move,  then 


A9    =  (Ae)2,    +  v^  .  t^^  (2.5) 


*ab  =  (^/^d^        -  (^^)2t  ^  (2.6) 

a 


where    (^9)2t     denotes  the  joint  displacement  in  the  start-up  and  set- 
a 

down  segments.    We  can  derive    (^9).       from  Eqn.  (2.3)  and  Figure  2.1 

"a 

as 


a 


^max  4nst  '  ^(^acc  '  ^const^  '  ^acc^  (^.7) 


In  Eqn.  (2.7), 


^acc  =  ^max/"^  (2.8) 


^const  ~  (^a  '2  t^^^) 


(2.9) 
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^(^acc)  =        W  ^cc  (2.10) 


Thus,  from  Eqns.  (2.4)  and  (2.6),  the  move  time  for  each  joint  can 
be  determined  as  (2tg  +  t^i^).  The  joint  that  gives  the  maximum  move 
time  is  the  pacing  joint. 

Let  T^-  denote  the  move  time  for  the  pacing  joint  and  the  point-to- 
point  move.  In  order  to  coordinate  the  motion  of  all  the  joints,  the 
move  parameters  for  the  non-pacing  joints  must  be  determined  based  on 
the  total  move  time,  T^ .  An  accepted  and  the  most  frequently  used 
method  for  determination  of  the  move  parameters  of  the  non-pacing  joints 
is  to  determine  the  velocities  of  the  non-pacing  joints,  assuming  that 
each  joint  accelerates  to  its  maximum  acceleration  (tg  >  2Tg(,(.)  during 
start-up  (and  set-down)  [Paul  1979].  From  Eqn.  (2.4),  assuming  t^  > 
2*acc  ^^^^  equations  for  tg  =  21^^^  can  be  derived  similarly), 

^d  =  \ax        -  ^max/"^)  (2.12) 

Since  a^^g^,  m  are  known,  we  need  to  determine  tg  for  each  non- 
pacing  joint.  Expressing  (^9)2^-  of  Eqn.  (2.7)  in  terms  of  m,  tg  and 
a^^g^,  and  substituting  in  Eqn.  (2.5),  we  get 


(^^^npj  =  i  '  (^x/'"  '  ^max     )  tg  -  a^^^  T./m  (2.13) 

(Aax)  ^a  '  (^x/'"  '  W  ^i)  tg  -  a2^;T./m  -  (Ae)^^.  =  0  (2.14) 
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From  Eqn.  (2.14),  we  can  solve  for  t^.  We  then  evaluate  v^j  from 
Eqn.  (2.12).  Thus,  the  move  parameters  for  the  non-pacing  joints  can  be 
determined  from  m,  a^^g^,      ,  and  A9. 

The  determination  of  t^  by  Eqn.  (2.14)  and  v^  by  Eqn.  (2.12)  clips 
the  velocities  (from  the  user  specified  percentage)  of  the  non-pacing 
joints,  without  changing  m  and  a^a^*  Since  some  of  the  non-pacing 
joints  may  have  smaller  displacements  and  are  faster  than  the  pacing 
joint,  the  start-up  and  set-down  of  these  joints  can  be  made  slower  than 
the  pacing  joint.  The  slower  start-up  of  some  of  the  non-pacing  joints 
smooths  the  initial  jump  in  the  end  effector.  This  may  be  significant 
for  a  delicate  tool  attachment  at  the  end  effector.  We  now  develop  a 
simple  technique  for  the  determination  of  a  factor  for  the  slower  start- 
up of  the  non-pacing  joints.  This  factor  is  a  smaller  value  of  "m"  for 
each  non-pacing  joint. 

Since  we  now  introduce  an  additional  variable  "m"  in  the 
determination  of  the  non-pacing  joint  parameters,  we  need  an  extra 
equation  or  constraints  for  a  unique  solution.  We  realize  the  extra 
information  by  imposing  the  constraint  that  the  non-pacing  joints 
accelerate,  cruise  and  decelerate  to  a  stop  for  the  same  periods  of  time 
as  the  pacing  joint.  In  other  words,  we  coordinate  the  motion  of  the 
joints  during  the  start-up  segment,  cruise  segment  and  the  set-down 
segment.  This  constraint  improves  the  esthetics  of  the  motion,  and  as 
shown  later,  reduces  the  initial  jerk  in  the  end  effector.  We  note  that 
for  a  given  move,  there  may  be  one  or  more  non-pacing  joints  that  will 
not  yield  the  desired  displacement  with  the  above  constraint.  We  now 
derive  the  condition  for  coordinating  the  motion  of  a  non-pacing  joint 
during  start-up,  cruise,  and  set-down  segments  of  the  pacing  joint.  Let 
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m'  denote  the  new  start-up  slope  for  a  non-pacing  joint.  From  Eqn. 
(2.13),  substituting  m'  for  m,  we  can  derive  an  equation  for  m'  in  terms 
°^  ^a*        ^max'  (Ae)^^.  as 

=  -  Ti)}  /  {(Ae),pj  -  a^^^  t^  (T.  -t^)}  (2.15) 

If  m'  is  less  than  m,  then  the  non-pacing  joint  motion  can  be 
coordinated  with  the  motion  of  the  pacing  joint  during  the  3  segments 
with  a  slower  start-up  and  set-down.  If  m'  is  greater  than  m,  then  the 
non-pacing  joint  cannot  be  coordinated  during  start-up,  cruise,  and  set- 
down  of  the  pacing  joint.  In  this  case,  the  move  parameters  for  the 
non-pacing  joint  are  determined  from  Eqns.  (2.12)  and  (2.14).  This  case 
can  occur  when  the  maximum  acceleration  of  the  non-pacing  servo  is  less 
than  the  maximum  acceleration  of  the  pacing  servo,  and  the  displacements 
of  the  two  joints  are  comparable.  Thus,  by  employing  Eqn.  (2.15),  we 
can  determine  the  move  parameters  of  the  non-pacing  joints,  which  are 
considerably  simplified  when  m'  <  m,  since  t^  and  t^j^  are  known  from  the 
pacing  joint  parameters. 

In  the  above  paragraphs,  we  developed  a  model  for  point-to-point 
joint  moves  with  cubic  polynomials  for  start-up  and  set-down.  For 
manipulators  with  heavily  damped  servos,  t^^^  may  be  specified  as  0 
which  makes  the  start-up  and  set-down  trajectories  quadratic 
polynomials.  For  a  quadratic  start-up,  by  imposing  the  constraint  that 
the  start-up  time  of  a  non-pacing  joint  is  the  same  as  that  of  the 
pacing  joint,  we  can  reduce  the  initial  jump  in  the  acceleration  of  the 
non-pacing  joints.  If  k.a^^g^  denotes  the  acceleration  of  a  non-pacing 
joint,  then  k  can  be  derived  as 
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k  =  (Ae)    .  /  {a      t    (t  +  t  .  )}  (2.16) 

Note  that  k  =  1  for  the  pacing  joint.  If  k  <  1  for  a  non-pacing 
joint,  then  start-up,  cruise,  and  set-down  times  of  the  pacing  joint  can 
be  employed  for  the  non-pacing  joint,  with  the  non-pacing  joint 
acceleration  clipped  to  k.a^g^^,  k  <  1.  If  k  >  1  for  a  non-pacing  joint, 
then  the  joint  trajectory  is  planned  such  that  the  start-up  and  set-down 
times  are  determined  independently  of  the  pacing  joint,  and  only  the 
cruise  velocity  is  clipped  such  that  the  move  time  is  . 

In  order  to  illustrate  the  acceleration  clipping  factor  "k"  of  Eqn. 
(2.16),  Table  2.1b  shows  the  value  of  k  for  each  joint  for  a  number  of 
point-to-point  joint  moves  of  Table  2.1a.  The  manipulator  of  Figure  2.2 
is  used  for  simulation.  The  maximum  joint  velocities  and  the  maximum 
joint  accelerations  for  the  manipulator  of  Figure  2.2  are  shown  in  Table 
2.2.  Note  that  for  all  the  moves,  the  acceleration  can  be  clipped  for 
most  or  all  of  the  non-pacing  joints.  The  clipping  can  be  greater  than 
9ff/^  of  the  maximum  joint  acceleration  for  some  non-pacing  joints. 

In  the  following  sections,  we  describe  the  on-line  determination  of 
joint  trajectories  for  controlled  motions  of  the  robot  end  effector 
Cartesian  space. 

Constrained  Motion  of  the  End  Effector  — 
Polynomial  Approximation  of  Joint  Trajectories 

In  a  constrained  end  effector  motion,  the  trajectory  of  each  joint 
must  be  controlled  to  result  in  the  desired  trajectory  of  the  end  effec- 
tor. The  use  of  polynomial  functions  for  generating  accurate  and  smooth 
joint  trajectories  is  a  well  known  technique  [Finkel  1976,  Lewis  1974, 
Luh  et  al .  1983,  Paul  1972].    It  has  been  shown  that  a  large  number  of 
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Table  2.1    Start  Point,  Stop  Point,  and  Move  Velocity 


MOVE 
# 

START  POSITION  (DEG.) 

(61,62,63, 64,65, eg) 

DESTINATION  (DEG.) 
(®1'^2'^3'®4'^5'®6^ 

% 

VELOCITY 

1 

0,0,0,0,0,0 

30,45,45,20,50,40 

70 

2 

10,25,30,45,60,75 

80,45,45,45,45,45 

50 

3 

10,10,10,10,10,10 

60,60,60,60,60,60 

70 

4 

10,10,10,10,10,10 

60,70,100,60,60,60 

40 

5 

10,20,30,45,45,45 

-20,-20,-60,-40,-40,-40 

30 

6 

10,20,30,45,45,45 

-100,-45,-45,70,70,70 

100 

7 

10,20,30,45,45,45 

10,30,40,-45,-45-,-45 

60 

8 

60,30,40,-45,10,50 

-10,50,-20,-10,50,70 

80 

9 

50,10,-45,60,30,40 

70,50,-10,-20,50,-10 

60 

10 

_45, -45, .45, 60, 60, 60 

30,50,60,-30,-30,-30 

70 
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TABLE  2.2    Acceleration  Factors 


MOVE 
# 

MOVE 
^acc 

TIME 
^const 

(SECS.) 

k2 

^3 

k4 

^5 

1 

0.183 

2.321 

2.686 

0.716 

1 

0.882 

0.199 

0.464 

0.375 

2 

0.165 

4.456 

4.787 

1 

0.266 

0.176 

0 

0.083 

0.168 

3 

-.183 

2.599 

2.965 

1.073 

1 

0.882 

0.448 

0.418 

0.422 

4 

0.104 

5.737 

5.945 

0.894 

1 

1.121 

0.373 

0.348 

0.351 

5 

0.125 

6.335 

6.585 

0.406 

0.504 

1 

0.48 

0.447 

0.452 

6 

0.33 

3.3 

3.962 

1 

0.55 

0.56 

0.094 

0.088 

0.089 

7 

0.185 

1.905 

2.275 

0 

0.263 

0.232 

1.062 

0.99 

1 

8 

0.264 

2.624 

3.153 

1 

0.266 

0.704 

0.208 

0.222 

0.112 

9      0.156      2.439      2.753    0.537       1       0.772    0.897    0.209  0.527 


10     0.183      5.103      5.468    0.847       1       0.975    0.425    0.396  0.4 
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Table  2.3    Maximum  Joint  Velocities  and  Maximum  Joint 
Accelerations  for  the  Robot  of  Figure  2.2 


JOINT  ANGLE 

MAXIMUM 
JOINT  VELOCITY  (rad/sec) 

MAXIMUM 
JOINT  ACCL.  (rad/sec^) 

0.515 

1.5575 

02 

0.4365 

1.5725 

^3 

0.7595 

1.896 

^4 

1.257 

3.728 

^5 

1.24 

4.00 

^6 

1.22 

3.961 
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Figure  2.2 


Kinematic  structure  of  robot  used 
in  simulation 
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undesirable  properties  of  high  order  polynomials  can  be  overcome  by 
using  lower  order  spline  functions  [Finkel  1976].  If  a  trajectory  has  a 
large  number  of  points,  then  a  spline  fit  for  the  trajectory  requires 
all  spline  coefficients  to  be  computed  before  the  trajectory  is 
executed.  This  is  due  to  the  inherent  nature  of  spline  calculations 
[Ahlberg  et  al .  1967,  DeBoor  1978],  Hence,  a  spline  trajectory  with  a 
large  number  of  intermediate  points  has  to  be  computed  off-line. 
Techniques  for  piece-wise  splines  using  X-splines  [Clenshaw  and  Negus 
1978]  have  also  appeared  in  literature  [Lin  and  Chang  1983].  In  the 
following  sections,  we  investigate  the  ideal  number  of  look-ahead  points 
for  a  piece-wise  cubic  spline  trajectory  and  describe  two  novel 
techniques  for  breaking  the  off-line  spline  computations  into  smaller 
blocks  for  real-time  trajectory  calculations. 

An  end  effector  move  is  executed  by  transforming  the  end  effector 
position  and  orientation  into  joint  coordinates  at  a  large  number  of 
points  on  the  end  effector  trajectory.  If  the  move  is  to  be  in  real 
time  with  on-line  trajectory  calculation,  we  look-ahead  a  few  points  on 
the  end  effector  trajectory  and  generate  the  joint  trajectories.  The 
complexity  of  the  interpolation  scheme  used  to  approximate  the  joint 
trajectories  directly  depends  on  the  time  interval  between  successive 
joint  coordinates,  i.e.,  the  sampling  rate  on  the  end  effector 
trajectory.  If  the  sampling  rate  is  high,  then  a  polynomial  of  degree  1 
may  be  adequate.  For  slower  sampling  rates,  linear  interpolation  of 
joint  coordinates  will  produce  jerky  motion  and  may  even  cause 
instability  [Paul  1981].  In  order  to  demonstate  a  typical  joint 
trajectory  during  a  constrained  end  effector  motion,  a  plot  of  joint 
displacement  versus  time  for  a  joint  angle  of  a  5-degrees  of  freedom 
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robot  for  one  particular  move  is  shown  in  Figure  2.3.  Joint  target 
points  for  the  elbow  rotation  (angle  6^)  were  generated  at  equal  time 
intervals  for  a  Cartesian  move  of  the  end  effector  of  a  robot  whose 
kinematic  structure  is  shown  in  Figure  2.2.  A  polynomial  of  degree  one 
(straight  line)  is  used  for  interpolation  between  successive  target 
points.  This  form  of  interpolation  can  be  unsatisfactory  for  a  smooth 
servo  response  because  a  linear  function  in  displacement  produces 
impulsive  accelerations  between  constant  velocity  segments. 

A  well  known  technique  for  smoothing  a  joint  trajectory  is  to  use 
polynomials  of  a  degree  higher  than  1.  A  single  polynomial  fit  through 
all  the  intermediate  points  of  a  joint  trajectory  results  in  an 
extremely  high  degree  for  the  polynomial.  For  instance,  if  the  joint 
trajectory  has  n  intermediate  points,  the  minimum  degree  of  one 
polynomial  that  passes  through  all  the  points  is  n-1  (Lagrange  interpo- 
lation). Splining  low  order  polynomials  provides  an  elegant  way  of 
reducing  the  overall  degree  and  also  the  computational  burden  of  trajec- 
tory generation.  The  degree  of  the  polynomial  should  be  such  that  the 
overshoot  and  wandering  are  minimal  [Finkel  1976,  Lewis  1974]. 
Overshoot  occurs  when  the  value  of  the  polynomial  goes  beyond  the  "band" 
of  initial  and  final  values.  Overshoot  becomes  obvious  for  a  quartic 
polynomial  when  the  "band"  between  start  and  end  point  is  small. 
Wandering  is  retrograde  motion.  Both  overshoot  and  wandering  are 
undesi  rable. 

A  number  of  papers  in  recent  literature  have  described  the 
application  of  spline  functions  to  the  generation  of  smooth  robot  joint 
trajectories  [Luh  et  al .  1983].  Splined  polynomials  are  used  for  point- 
to-point    joint    motion    and    motion    involving    a    large    number  of 
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intermediate  joint  positions.  [Luh  et  al .  1983]  describes  the 
construction  of  cubic  spline  joint  trajectories  with  optimum  placement 
of  the  knots.  The  cubic  splines  are  constructed  off-line  with  a 
knowledge  of  the  end  effector  trajectory.  A  classification  of  schemes 
using  splined  polynomials  for  constructing  a  joint  trajectory  with  a 
large  number  of  knots  is  as  follows: 

(1)  4-3-3—4  trajectory 

The  first  segment  is  a  4th  order  polynomial  specifying  the 
trajectory  from  initial  to  the  "lift-off"  position  of  the  joint  [Paul 
1981].  The  following  polynomials  are  all  cubics  except  for  the  last 
trajectory  segment  which  is  a  quartic. 

Consider  the  joint  displacement  trajectory  shown  in  Figure  2.3.  At 
the  initial  and  destination  points,  the  position,  velocity  and 
acceleration  are  known.  At  each  intermediate  point,  the  position  or 
joint  displacement  is  known.  To  make  the  interpolation  curve  doubly 
continuous  at  all  points,  the  constraint  that  acceleration  be  continuous 
at  the  intermediate  points  where  2  polynomials  join  is  imposed.  Using 
this  constraint  at  the  intermediate  points,  a  set  of  equations  are  gen- 
erated that  determine  the  slope  of  the  curve  at  the  intermediate  points 
where  2  polynomials  are  splined  together  [Ahlberg  et  al .  1967,  DeBoor 
1978].    A  formulation  of  this  type  results  in  a  4-3-3—4  trajectory. 

(2)  3-3—3  trajectory 

This  is  a  cubic  spline  fit  for  all  the  trajectory  segments.  The 
only  difference  from  the  4-3-3—4  scheme  is  that  the  acceleration 
constraints  at  the  start  and  destination  points  are  ignored  to  yield  all 
cubic  polynomials. 
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Inherent  in  the  spline  formulation  is  determination  of  the  slopes 
of  the  interpolating  polynomial  at  all  intermediate  points  between  start 
point  and  end  point.  Thus,  if  there  are  n  intermediate  points,  it  is 
necessary  to  solve  a  matrix  equation  of  dimension  nXn.  The  equation 
involves  solution  of  a  tradiagonal  matrix,  which  is  uniquely  solvable  by 
the  L-U  decomposition  scheme  [Johnson  1977],  A  typical  motion 
trajectory  of  the  end  effector  might  result  in  50  or  more  intermediate 
points  for  a  joint  trajectory.  Solution  of  a  matrix  equation  of 
dimension  50x50  is  impractical  both  in  terms  of  computational  time  and 
storage  requirements  and  is  feasible  only  for  an  off-line  trajectory 
planning  scheme.  For  many  robot  applications,  an  off-line  generated 
trajectory  is  unacceptable.  A  specific  example  is  sensor  based  control 
of  the  robot  end  effector,  where  the  trajectory  cannot  be  pre-planned. 
In  this  case,  splines  for  the  joint  trajectories  must  be  generated  on- 
line. Two  schemes  employing  cubic  splines  for  approximating  joint 
trajectories  with  limited  point  look-ahead  are  derived  in  this 
chapter.  Experimentation  has  shown  that  (1)  the  on-line  cubic  splines 
provide  smooth  motion  of  the  joints  (and  end  effector)  for  a  wide  range 
of  sampling  rates,  and  (2)  the  computations  that  determine  the  required 
spline  coefficients  are  simple  enough  for  real  time  implementation. 
Also,  simulation  has  shown  that  for  slower  sampling  rates,  the  splines 
track  the  end  effector  trajectory  much  better  than  a  simple  linear 
interpolation. 

The  following  sections  of  the  chapter  are  organized  as  follows. 
The  next  section  derives  an  "ideal"  number  of  look-ahead  points  for 
generating  cubic  spline  trajectories  on-line.  We  then  develop 
techniques  for  estimating  an  additional  constraint  at  the  last  point  of 
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a  look-ahead  block.  An  analytical  derivation  of  the  acceleration  jump 
in  the  on-line  splines  scheme  is  also  given.  Finally,  the  results  of 
experimentation  and  simulation  are  described. 

Effect  of  Error  in  Estimation  of  an  Intermediate  Velocity 
on  Preceding  Velocities  in  a  Cubic  Splines  Trajectory 

In  order  to  compute  splines  for  a  joint  trajectory  on-line,  it  is 
necessary  to  look  ahead  a  few  points  on  the  end  effector  trajectory. 
The  current  point  and  the  look-ahead  points  are  referred  to  as  a 
"block,"  the  block  size  being  determined  by  the  number  of  points  in  the 
block.  Ideally,  the  size  of  a  block  should  be  such  that  an  error  in 
estimation  of  an  additional  constraint  at  the  last  point  of  the  block 
should  be  localized  to  the  block  itself.  For  most  robot  controllers, 
the  block  size  is  fixed  by  the  computational  bandwidth  of  the  controller 
for  the  determination  of  the  look-ahead  points  on  the  end  effector 
trajectory.  This  section  describes  a  method  for  the  estimation  of  an 
ideal  block  size,  which  can  be  implemented  if  the  controller  provides 
the  flexibility  for  changing  the  block  size. 

Figure  2.4  shows  a  segment  of  an  example  joint  trajectory  with  the 
start  point  PI,  and  two  intermediate  points  P2  and  P3.  The  block  size 
in  Figure  2.4  is  assumed  to  be  3  (2  point  look-ahead).  Two  cubic 
polynomials  with  acceleration  continuity  at  the  intermediate  point  P2 
can  be  evaluated  for  the  present  point  PI  and  the  two  look-ahead  points 
P2  and  P3,  if  one  additional  constraint  at  P3  is  known.  At  PI,  the 
position  and  velocity  are  known,  at  P2  and  P3  the  only  information 
available  is  the  position  and  63).  By  splining  2  polynomials  with 
continuity  in  acceleration  at  P2,  the  velocity  at  P2  can  be  determined 
[DeBoor  1981].    Therefore,  with  4  constraints  between  PI  and  P2,  a  cubic 
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Figure  2.4 


Sample  points  on  a  joint  trajectory 
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polynomial  for  the  trajectory  P1-P2  can  be  computed.  The  polynomial  P2- 
P3,  however,  will  be  a  quadratic  since  only  one  boundary  condition 
(position)  is  known  at  the  point  P3.  If  an  additional  constraint  at  the 
point  P3  (velocity  or  slope  of  the  curve)  is  estimated,  then  a  cubic 
polynomial  can  be  evaluated  for  the  segment  P2-P3.  This  sections 
studies  the  effect  of  the  velocity  at  the  block  end  point  (P3  in  the 
example  of  Fig.  2.4)  on  the  velocities  at  the  preceding  points. 

An  all  cubic  splines  (3-3--3)  trajectory  with  n  intermediate  points 
yields  a  tri-diagonal  matrix  equation  for  the  determination  of  the 
slopes  v^,Vp,...,Vp  at  the  n  intermediate  points  [DeBoor  1978,  Luh  et 
al.  1983]. 
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where  the  coefficients  a^j  and  are 
^i.i      =  2  MTi  +  T^^i) 
^i,i-l  "  Vl 
^i,i+l  "  """i 

bi        =  (3  *  Vi/Ti)  *  (0^  -  G^.i)  + 

(3  *  T^-/Vi)  *  (e^.^i  -  e^.)  (2.18) 

with  T^-,  being  the  ith  and  the  (i+l)th  time  intervals,  9^  the  ith 

joint  position  at  time  t^   and  Vq  and  are  the  velocities  at  the 

start  and  destination  points  (see  Ahlberg  et  al .  1967,  Luh  et  al .  1983) 
for  details  on  the  derivation  of  Eqn.  (2.17)).  The  matrix  equation  (Eqn. 
(2.17))  is  derived  by  imposing  continuity  in  acceleration  at  the  n 
intermediate  points  [Ahlberg  et  al .  1967,  DeBoor  1978].    This  equation 

can  be  uniquely  solved  for  y^,^^  recursively  [Johnson  1977].  We 

will  show  that  if  Eqn.  (2.17)  is  broken  into  a  number  of  smaller 
segments  for  piece-wise  splines,  then  an  error  in  estimation  of  the 
velocity  at  the  end  of  each  smaller  computational  block  affects  the 
slopes  at  only  a  few  preceding  points,  beyond  which  the  error,  as 
defined  later  by  Eqn.  (2.26),  becomes  negligible.  It  will  be  shown  that 
for  a  cubic  splines  trajectory  with  equidistant  knots,  an  error  in 
estimation  of  an  intermediate  velocity  causes  errors  of  significant 
magnitude  (in  decreasing  order)  only  in  the  4  preceding  velocities. 
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A  recursive  solution  to  (2.17)  can  be  derived  as  follows: 


Define  Yj  =  ajj+^/3j  j  =  1, n-1 


^1  =  an  (2.19) 

Also  define 


Zl  =  (bi  -  Vq  Ti)/3i 

then 

=       -  Vl  J  =  (2.20) 

where  a^j  denotes  the  element  of  the  ith  row  and  jth  column  of  the  nXn 
matrix  on  the  left  hand  side  of  Eqn.  (2.17)  and  bj  denotes  the  jth 
element  of  the  vector  on  the  right  hand  side  of  Eqn.  (2.17). 

We  will  now  derive  the  intermediate  velocities  v-^  ...  v^  in  terms 
of  the  last  velocity,  v^^,  and  other  terms  involving  zj  and  y j .  The 
equations  so  derived  will  demonstrate  the  effect  of  a  change  or  error  in 
the  velocity  v^^  on  the  intermediate  velocities. 
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=  zi  -  Yi  Z2  +  Ti  Y2  Z3  -  ... 
+  {-!)"  Yn  Yp.i  Yn.2  ...  Yi  (2.21) 


Now,  if  v^j  becomes  +  Av^^  then  the  magnitude  of  the  error 

in       is  given  by  Avj.  as  follows: 

=  (^Vl)  ^n  Vl  •••  ^1  (2.22) 
=  (^Vl)  (^n/^n)  (Vl/Vl)  •••  (Ti/3i) 

we  now  show  that  the  product  of  the  terms  on  the  right  hand  side  of  Eqn. 
(2.22)  becomes  0  in  the  limit  as  "n"  approaches  infinity.  This  is 
easily  demonstrated  by  showing  that  0  <  (T^-ZP-j)  <  1  for  i  =  1  to  n. 
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=  2(1  +  yi^) 


(2.23) 


Since  T2  and  Tj  are  both  positive,  ^iHi  is  positive  and  greater 
than  1.    From  (2.23)  and  (2.19) 


(2.24) 


From  (2.23)  and  (2.24)  $2/^2  ""s  also  greater  than  1.    By  induction. 


is  also  greater  than  1.  Therefore,  the  right  hand  side  of  Eqn.  (2.22) 
progressively  reduces  in  magnitude  with  increasing  "n".  From  Eqn. 
(2.22)  we  define  a  memory  error  factor  yj  >  0  as  a  measure  of  the 
influence  of  a  change  in  the  slope  at  the  (rH-l)st  knot  on  the  slope  at 
the  jth  knot  (j  <  (rn-l)). 


hence     Av .  =  y .  (Av    , ) 


Since     lim    y.  =  0,  a    practical    threshold    value    of    y^    can  be 
defined  for  a  given  application  such  that  for  some  finite  j,  the  error 


3^/1^  =  2  (1  +  (T.^i/T.)  *  (1  -  0.5/(e._^/T._^))) 


(2.25) 


n 


K-J 


(2.26) 
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in  estimating  the  velocity  at  the  (rH-l)st  knot  has  no  practical  effect 
on  the  jth  knot.  In  this  chapter,  we  assume  the  system  memory  error  is 
negligible  when  p-^  <  e  and  e  is  less  than  0.5%.  Thus,  given  the 
sampling  intervals,  we  can  determine  an  "ideal"  block  size  (n)  such  that 
the  error  due  to  estimation  of  the  slope  at  the  last  point  of  the  block 
is  localized  to  the  block  itself  namely  <  e,  where  e  >  0  provides 
some  prescribed  level  of  performance  in  trajectory  motion.  The  next 
section  applies  these  results  to  a  cubic  splines  trajectory  with 
equidistant  knots. 

Application  to  Cubic  Splines  with  Equidistant  Knots 

If  the  knots  of  a  joint  trajectory  are  equidistant  in  time,  then 
the  elements  of  the  matrix  in  Eqn.  (2.17)  simplify  as  follows: 


^i,i=         '    '  i-l,i=  '  i,i.f  T  (2.27) 


Substituting  for  a^- j  in  the  expressions  for  y j ,  3j  and  Zj  in  Eqn, 
(2.19)  we  obtain 

Yj  =  T/3j  j  =  n-l,...,l 


3j  =  4T  -  T2/3j.i 


3i  =  4T 

^j  =  (^j  -  ^j-l)/^j 
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j  =  n-l,...,2  (2.28) 

Now      can  be  computed  from  (2.28)  as  follows: 

=  4T,  32  =  3.75T,  33  =  3.73343T, 

34  =  3.73218T,  35  =  3.73218T,...,3n.i  =  3.73218T 

Note  that  3p  appears  to  converge  to  the  value  3.73218T  for  n  >  4. 
We  now  show  analytically  that  3^  always  converges  to  the  value  (2  +  /3)T 
for  a  cubic  splines  interpolation  with  equidistant  knots. 

Consider 


where 

3*  =  (2  +  /3)T  (2.30) 


B2  =  (2  -  /3)T  (2.31) 
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From  Eqn.  (2.29),  it  is  seen  that  the  B  sequence  is  monotone 
decreasing  for  B^_^  >  or  3^_-^  <  B^  •  Since  B^  =  B^  (Eqns.  (2.28) 
and  (2.30)),  the  B  sequence  is  monotone  decreasing,  and 


^  =       =  (2  +  /3)T  (2.32) 


Since  y-  =  T/B.  , 

J  J 


=  T/(2  +  /5)T  =  2-/3  (2.33) 


From  Eqn.  (2.26),  it  is  seen  that  the  effect  of  a  change  in  the 
velocity  of  the  last  point  (^v^^)  °"  velocity  of  the  i  th  point 
(v^-)  is  succintly  described  by  y.  =  (Av./Av^^),  a  value  only  dependent 
upon  i  <  n.  An  example  trajectory  with  10  points  is  used  to  demonstrate 
the  influence  of  a  change  in  the  last  velocity  on  the  preceding 
velocities.  Initially,  it  is  assumed  that  the  position  and  slope  of  the 
trajectory  at  the  start  and  final  points  are  known.  At  the  eight 
intermediate  locations,  only  the  position  coordinate  is  known.  Table 
2.3  shows  the  changes  in  the  velocities  of  the  eight  intermediate  points 
due  to  a  change  Avg  in  the  last  (tenth)  velocity.  The  table  shows  that 
a  change  in  the  velocity  of  any  point  on  the  cubic  spline  trajectory 
results  in  significant  changes  only  in  the  preceding  4  velocities, 
beyond  which,  the  effect  of  the  change  becomes  insignificant,  less  than 
0.5%  of  the  change. 
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TABLE  2.4    Memory  error  factor  for  cubic  splines 

with  equidistant  knots  (n=8,  Av^-  =  y^-  Avg) 


i 

U-i 

^1 

8 

0.26794 

7 

0.07179 

0 

u  .uiy<i  J 

5 

0.00515 

4 

0.00138 

3 

0.00136 

2 

0.00009 

1 

0.00002 

The  numbers  in  Table  2.3  provide  an  important  general  result.  They 
show  that  for  an  all  cubic  splines  interpolation  with  equidistant  knots, 
an  error  or  change  in  the  slope  at  a  look-ahead  point  induces  relatively 
significant  errors  (yj  >  0.5%)  for  only  the  preceding  4  points.  The 
slopes  can  be  considered  unaffected  for  all  earlier  points.  The  memory 
of  the  interpolation  is  said  to  be  4.  This  result  can  also  be 
interpreted  the  following  way.  If  the  off-line  splines  calculations  are 
broken  into  blocks  of  6  points  each,  and  we  estimate  the  velocity  at 
every  sixth  point,  then  the  error  in  estimation  of  the  velocity  is 
limited  to  errors  in  its  block  of  4  intermediate  points. 
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The  next  section  describes  schemes  for  estimating  the  slope  of  the 

trajectory  at  certain  intermediate  points  for  on-line  spline 
computations  with  limited  point  look-ahead. 

On-Line  Cubic  Splines  for  Joint  Trajectory  Interpolation 

Scheme  1 

One  way  of  determining  an  additional  constraint  at  the  last  point 
of  a  look-ahead  block  is  to  compute  the  joint  velocities  by  an  "inverse 
velocity"  transformation.     There  are  several  approaches  to  an  inverse 
velocity  determination.      Two   general    approaches   are  the   use   of  an 
inverse  Jacobean  matrix  [Whitney  1969],  and  Screw  theory  [Lipkin  and 
Duffy  1982].    The  data  required  for  an  inverse  velocity  transformation 
are  end  effector  linear  and  rotational  velocities  and  the  configuration 
of  the  arm.     The  complexity  of  the  inverse  velocity  transformation  is 
dependent  on  the  geometry  of  the  manipulator.     For  many  6  degrees  of 
freedom  robots  with  intersecting  axes  at  the  wrist  (spherical  wrist), 
the  inverse  velocity  computation  can  be  simplified  [Featherstone  1983, 
Hollerbach  and   Sahar   1983].      Since  position   and   orientation   can  be 
decoupled  at  the  wrist  we  can  determine  the  velocities  of  the  first  3 
joints  in  one  step  and  the  velocities  of  the  latter  3  joints  in  the  next 
step.     Since  the  reverse  velocity  determination  is  not  computationally 
intensive  for  the  class  of  manipulators  with  spherical   wrists,   it  is 
possible  to  determine  the  splines  for  each  look-ahead  block  in  real- 
time. 

With  position  and  velocity  constraints  at  the  start  and  end  points 
of  a  block,  velocities  at . each  intermediate  point  of  the  block  can  be 
estimated  by  imposing  continuity  in  acceleration  and  solving  the  tri- 
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diagonal  matrix  equation.  If  the  block  size  is  small,  no  matrix 
manipulations  are  involved  since  the  equations  for  the  intermediate 
velocities  can  be  expressed  in  closed  form.  Thus,  with  position  and 
velocity  constraints  at  each  intermediate  point  of  the  joint  trajectory, 
and  ignoring  the  acceleration  at  the  start  and  final  points,  cubic 
polynomials  can  be  fit  for  the  entire  trajectory.  The  first  and  last 
polynomials  will  be  quartics  if  initial  and  final  point  acceleration  is 
considered.  The  interpolating  curve  will  be  continuous  in  the  first  and 
second  derivatives  at  all  points  except  the  block  end  points  where 
continuity  in  second  derivatives  is  lost.  We  now  briefly  outline  the 
equations  for  generating  the  spline  coefficients  for  each  block.  For 
simplicity,  a  2-point  look-ahead  is  assumed. 

A  normalized  time  "u"  for  each  segment  of  the  joint  trajectory  is 
defined  as  follows: 

For  the  ith  segment,  let 


u  =  (t  -  ti.i)/(Ti.i) 


where 


i-1 


=  t.  -  t. 


i-1 


(2.34) 


All  polynomials  are  expressed  with  respect  to  u. 


du/dt  =  1/T^_2    on  time  segment  i 


(2.35) 


Let 


fi  (u)  =  a3^  u^  +  a2T  u^  +  a^i  u  +  ag-j 


(2.36) 


For  the  ith  segment,  f^(u)  is  evaluated  from  u  =  0  to  u  =  1. 
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df^(u)/du  =  (dfi(u)/dt)/(du/dt) 

=  T^.i  *  (dfi(u)/dt)  (2.37) 

Also,       d^  (f^-(u))/du2  =  t2._^  *  (d2f.(u)/dt2)  (2.38) 

Equations  (2.37)  and  (2.38)  transform  the  velocity  and  acceleration 

of  the  joint  at  t^  to  velocity  and  acceleration  of  the  normalized 
trajectory. 

Let  f^(u)  =            ^  ^21      +        u  +  agi  =  0  (2.39) 

and  f2(u)  =            +            +  3^2  "     ^22  =  0  (2.40) 


be  the  cubic  polynomials  describing  the  trajectory  between  and  P2, 
and  P2  and  P3  in  Figure  2.4. 

Let  9]^,  63  denote  the  velocities  at  P^  and  P3  respectively.  We 
know  from  the  polynomial  preceding  P^.  If  P^  is  the  start  point  of 
the  trajectory  and  the  arm  starts  from  rest,  then  6^  =  0.  We  can 
determine  63  by  a  "reverse  velocity"  transformation  at  time  instant  t3 
(point  P3).  Splining  (2.39)  and  (2.40)  together  with  continuity  in 
acceleration  at  P2,  we  derive  an  expression  for  §2  as  follows: 


(6a3j  u  +  2a2i)/T^  |    _  =  {6a^^  u  +  2a22)/T^ 


u  =  1 


(2.41) 

u  =  0 


Expressing  all  coefficients  in  (2.39)  and  (2.40)  in  terms  of  e^, 
®2»  ®3'         ^2'  ^3»  96t 
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aoi  =  ^1       '            =  eiTi  (2.42) 

=  3         -  e^)  -  (02  +  2ei)Ti  (2.43) 

=  -2  (^2  -  ^l)      (®1  ^  ^2)^1  (2.44) 

ao2  =  ^2      '       ai2  =  ^2'r2  (2.45) 

322  =  3  (63  -  62)  -  (63  +  262)12  (2.46) 

832  =  -2  (63  -  62)  +  (§2  +  63)12  (2.47) 
Substituting  in  Eqn.  (2.41)  and  solving  for  62 
02  =  1/2  (Ti  +  T2)  [(3T2/Ti)(62  -  6^)  + 

(3Ti/T2)(63  -  62)  -  61X2  -  6311]  (2.48) 


The  coefficients  of  the  two  cubic  polynomials  are  computed  from  Eqns. 
(2.42)  through  (2.47). 

An  advantage  of  using  actual  joint  velocities  as  constraints  at 
certain  intermediate  points  is  that  each  joint  will  closely  track  the 
actual  velocity  required  of  it  to  maintain  a  desired  end  effector 
velocity.  This  implies  that  the  velocity  profile  of  the  end  effector 
between  intermediate  points  and  at  intermediate  points  will  be  closer  to 
the  desired  value.  As  mentioned,  acceleration  continuity  is  lost  at  the 
block  end  points. 
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For  some  manipulator  geometries,  an  inverse  velocity  computation 
may  be  computationally  too  intensive  for  a  limited  point  look-ahead,  on- 
line trajectory.  A  second  formulation  is  proposed  to  estimate  an 
additional  constraint  at  the  block  end  points.  The  computations  are 
independent  of  manipulator  geometry  and  are  simple  enough  for  on-line 
implementation. 

Scheme  2 

The  simplest  approach  to  determination  of  an  additional  constraint 
at  block  terminal  points  is  to  compute  joint  velocity  at  that  point  by  a 
linear  interpolation.  Thus,  at  the  point  P3,  in  Figure  2.4,  the  joint 
velocity  can  be  estimated  as 

P3'  =  (P3  -  P2)  /  T2  (2.49) 

Again,  in  this  case,  acceleration  will  be  discontinuous  at  the 
block  end  points  (including  the  start  and  final  points).  Since  the 
estimation  of  the  velocity  by  Eqn.  (2.49)  is  local  to  the  specific 
block,  the  tracking  error  will  be  larger.  The  "locality"  of  an 
estimation  of  this  type  is  illustrated  in  Figure  2.5.  Figure  2.5  shows 
a  plot  of  a  joint  trajectory  with  7  points.  The  5  intermediate  points 
are  spaced  equidistant  in  time.  The  joint  trajectory  is  for  joint  angle 
4  of  the  robot  shown  in  Figure  2.2  for  a  linear  end  effector  move.  The 
dashed  trajectory  is  obtained  by  computing  the  6  cubic  splines  by  the 
conventional  "off-line"  scheme.  The  slopes  at  the  5  intermediate  points 
for  the  off-line  scheme  were  determined  by  solving  a  5X5  tri -diagonal 
matrix.  The  solid  curve  is  obtained  by  estimating  velocities  at  every 
other  point  (2  point  look-ahead)  by  Eqn.  (2.49).    The  plots  demonstrate 
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the  tracking  error  resulting  due  to  a  poor  estimate  of  the  slope  (Eqn. 
(2.49))  at  the  intermediate  points. 

A  significant  improvement  in  the  estimate  of  the  slope  at  the  block 
end  points  is  achieved  by  the  following  technique.  The  idea  is  to 
estimate  the  slope  at  the  block  end  points  by  overlapping  successive 
blocks.  An  initial  estimate  of  the  slope  at  the  block  end  points  is 
made  by  assuming  the  last  polynomial  of  the  block  to  be  a  quadratic. 
This  slope  is  then  improved  by  "overlapping"  the  next  block  with  the 
current  block.  The  overlap  is  made  between  a  point  following  the  block 
end  point  and  the  point  preceding  the  block  end  point.  The  improved 
estimate  of  the  slope  at  the  block  end  point  is  obtained  by  splining  2 
polynomials  between  the  preceding  point,  block  end  point  and  the  next 
look-ahead  point  with  continuity  in  acceleration  at  the  block  end  point. 

Referring  to  Figure  2.4,  the  slope  of  the  curve  at  point  P2 
(midpoint  of  the  3  point  block)  is  initially  estimated.  Since  the  slope 
at  P3  is  not  known,  the  slope  at  P2  is  determined  independently  of  the 
slope  at  P3.  This  is  done  by  tying  together  a  cubic  polynomial  between 
PI  and  P2  and  a  quadratic  polynomial  between  P2  and  P3  with  continuity 
in  acceleration  at  P2.  Thus,  by  making  acceleration  continuous  at  P2, 
the  slope  of  the  curve  at  P2  is  determined  irrespective  of  the  slope  at 
P3.  Now  when  point  P4  is  known,  the  slope  at  P3  is  computed  again  using 
the  same  formulation  as  above.  This  new  slope  allows  a  cubic  polynomial 
to  be  fit  between  P2  and  P3  instead  of  a  quadratic.  Also,  the  new  slope 
at  P3  is  an  improvement  over  the  one  computed  by  differentiating  the 
quadratic.  Thus,  an  improvement  in  the  estimation  of  the  slope  is 
obtained  by  overlapping  successive  blocks.  Let 
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(u)  =  33^      +        ^   +  a-^-]^  u  +        =  0 


(2.50) 


f2  (u)  =        ^       ^12  ^      ^02  ~  ^ 


(2.51) 


be  the  equations  of  3  cubic  polynomial  between  -  P2  snd  3  qu3dratic 
polynomi3l  between  P2  -  P3  (Figure  2.4).  The  norm3lized  time  V3riable 
is  denoted  by  "u"  as  before.  The  initial  conditions  at  P-^,  P2  and  P3 
are  shown  in  Figure  2.4,  except  in  this  case,  there  is  one  less  boundary 
condition  at  P3;  th3t  is,  63  is  not  known  (recsll  th3t  in  Scheme  1,  63 
was  initially  determined  by  a  "reverse  velocity"  computation).  Hence, 
f2(u)  is  given  by  a  quadratic  equation  (2.51). 

Splining  (2.50)  and  (2.51)  together  with  continuity  in  acceleration 
at  P2,  82  is  determined  as  follows: 


6331U  +  2321/T2' 


=  2322/T2' 


(2.52) 


u  =  1 


u  =  0 


Expressing   3ll    coefficients    in    (2.50)    snd    (2.51)    in   terms  of 


^'  ^1'  ®2'  ^2'  ®3 
am  =  9 


01-^1      .     an  =  9iTi 


(2.53) 


321  =  3  (62  -  e^)  -  (201  +  e2)Ti 


(2.54) 


831  =  -2  (92  -  ^l)  +  ih  +  e2)Ti 


(2.55) 


a02  =  ^2      »      ^12  "  ^2^2  (2.56) 
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=  (^3  "        ■  ^2^2  (2.57) 
Substituting  in  Eqn.  (2.52),  we  evaluate  §2  as 

62  =  (T^T2/(2T2  +  T^)  *  [(1/T^)(e3  -  62)  +  (3/T^)(02  -  6^ )  -  Q^/J^l 

(2.58) 

When  point  is  known,  is  determined  by  splining  a  cubic 
polynomial  between  ?2  -  P3  and  a  quadratic  between  P3  -  P^. 

63  =  (1213/(2X3+  T2))  *  C(i/T^)(e4-  63)  +  (3/T^)(e3  -  62)  -  92/T2] 

(2.59) 

With  the  new  value  of  63  given  by  (2.59),  coefficients  of  a  cubic 
polynomial  between  P2  -  P3  are  computed  (instead  of  the  original 
quadratic)  as  follows: 

ao2  =  02     '     ^12  =  V2  (2.60) 

322  =  3  (63  -  62)  -  (202  +  63)12  (2.61) 

832  =  -2  (63  -  62)  +  (62  +  63)12  (2.62) 

Thus,  the  coefficients  of  the  cubic  polynomial  between  P^  -  P2  are 
determined  from  Eqns.  (2.53),  (2.54),  and  (2.55)  with  a  knowledge  of  6^, 
6^,  62  and  63;  and  the  coefficients  of  the  cubic  polynomial  between  Pp  - 
P3  are  determined  from  Eqns.  (2.60),  (2.61),  and  (2.62)  with  a  knowledge 
of  62,  62,  63,  83  and       (point  P4). 
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The  improved  tracking  accuracy  of  this  scheme  is  demonstrated  in 
Figure  2.6.  The  plots  of  Figure  2.6  show  the  same  joint  move  as  Figure 
2.5.  The  dashed  trajectory  is  the  "off-line"  spline  solution.  The 
solid  trajectory  is  obtained  by  a  2  point  look-ahead  on  the  joint 
trajectory  with  slope  determination  as  described  above. 

In  order  to  illustrate  the  improvement  in  trajectory  with  the 
number  of  look-ahead  points.  Figure  2.7  shows  a  plot  of  the  joint 
trajectory  of  Figures  2.5  and  2.6  with  a  4-point  look-ahead.  In  the 
preceding  section,  we  derived  that  a  look-ahead  of  4  points  for  an 
equidistant  knots  cubic  splines  trajectory  produces  a  system  memory 
error  of  less  than  0.5  percent.  We  deduce  from  Figure  2.7  that  the 
piece-wise  splines  track  the  off-line  splines  trajectory  with  almost  no 
undershoot  or  overshoot,  illustrating  better  tracking  due  to  negligible 
coupling  in  the  determination  of  the  velocities  at  the  block  end  points 
of  successive  computational  blocks. 

In  Eqn.  (2.59),  the  slope  at  P3  was  determined  with  the  overlap  in 
the  succeeding  block  limited  to  a  single  look-ahead  point  (P4).  With 
this  technique,  joint  trajectory  interpolation  can  start  with  a  2-point 
look-ahead  for  the  first  block,  after  which,  only  1  look-ahead  point  on 
the  trajectory  is  required  to  compute  the  next  joint  polynomial.  Thus, 
this  scheme  is  adaptable  to  sensor-based  control  where  smaller  look- 
ahead  means  quicker  adaptation  to  the  new  trajectory. 

The  estimate  of  9^  by  Eqn.  (2.59)  can  be  further  improved  by 
employing  both  points  of  the  next  look-ahead  block  (2-point  look-ahead) 
to  compute  6^  .  In  this  case,  we  spline  two  cubic  polynomials  between 
P2  -  P3  and  P3  -  P4  and  a  quadratic  polynomial  between  P^  -  Pg,  with 
continuity  in  the  second  derivative. 
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The  acceleration  continuity  equations  at  P3  and  are 


(6a.„u+  2a„)/T^  ,  =  (6a^„u  +  2a„)/T^  ,  (2.63) 

^'^     ^  \  w  =  \  "        |u  =  0 

(6a33U  +  2a23)/T^  =  2a24/T^  (2.64) 


Substituting  for  the  coefficients  a^-j  in  Eqn.  (2.63)  and 
simplifying,  we  get 


263  {l/T^)  +  (I/T3)}  +  yT3  =  (3/T^)  (e^  -  e^)  -  yi^  (2.65) 


Substituting  for  the  coefficients  a^j  in  Eqn.  (2.64)  and 
simplifying 

(63/13)  +  6^  {(2/T3)  +  I/T4)}  =  (1/T^)  (65  -  64)  -f  (3/T^)  (64  -  63) 

(2.66) 

Solving  for    63  and  9^     from  Eqns.  (2.65)  and  (2.66), 
63  =  [{T2T3  (T3  +  2T4)}  /  {2(T2  +  T3)  (T3  +  2T4)  -  T3T4}]  . 


[(3/T^)  (0^  -  e^)  -  {2J^n\  (13+  2T4)}  (64  -  63) 


{I/T4  (T3+  2T4)}  (65  -  e^)  -  l^n^l  (2.67) 


\  =  {V4/(T3+  2T4)}  [(1/T^)(e5  -  e^)  +  (3/T^)  (64  -  63)  -  63/13] 

(2.68) 
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We  note  that  the  overlap  correction  causes  a  discontinuity  in 
acceleration  due  to  the  improved  velocity  estimate  at  the  block  end 
point.  For  a  2-point  look-ahead,  and  velocity  estimations  by  Eqns. 
(2.58)  and  (2.59),  acceleration  is  discontinuous  at  the  block  end  point 
and  the  point  preceding  the  block  end  point.  If  the  velocity  at  the 
block  end  point  is  determined  by  Eqn.  (2.67),  then  acceleration 
continuity  is  lost  at  the  point  preceding  the  block  end  point.  In  the 
next  section,  we  show  that  the  discontinuity  in  acceleration  due  to  the 
on-line  cubic  splines  employing  overlap  velocity  correction  is,  to  a 
first  approximation,  always  smaller  than  the  acceleration  jumps  in  an 
on-line  cubic  splines  without  overlap  velocity  correction. 

Acceleration  Jump  in  On-line  Cubic  Splines 

The  acceleration  jump  at  the  "join"  point  of  two  cubic  polynomials, 
f^(u)  and  f^-+i(u)    is  given  as 

M.  =  [(6a3.u  -  2a2i)/T.2]^^^    -  [(6a3(.^^)U  -  2^2{nl)/l ,1^^ 

(2.69) 

where 

f.  (u)     =  a3.u'^  +  a2-u^  +  a^.u  +  a^.  (2.70) 

.      Vl(^)  =  ^3(i+l)  ^2(i+l)^^  ^  ^l(i+l)^  ^  ^O(in)  (2-^1) 

For    simplicity,    we    assume    equal     time    intervals;    that  is, 
-  T^.^^  =  T  .     Substituting  for  the  cubic  polynomial  coefficients  in 
terms  of    6,  6    (as  in  Eqns.  (2.42)  -  (2.44)),  in  Eqn.  (2.69),  we  obtain 
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[Aa].^^  =  (1/T^)  [-  6  (e.^^  -  e.)  +  (2e.  +  4e.^^)T  - 

6  (^2  -  Vl)  -  2  (^2^  2Vi)T]  (2.72) 
[Aa].,l  =  ^  [-  3  (9.^2  -        ^  T(9.  .  49.^^  .  9.^2^^  (2.73) 

Equation  (2.73)  gives  the  jump  in  acceleration  at  the  point  where 
two  cubic  polynomials  are  splined  together. 

In  order  to  estimate  the  acceleration  jump  in  the  on-line  cubic 
splines  approximation,  we  assume  that  the  actual  joint  trajectory  is 
continuous  and  nth  order  differentiable,  where  n  could  be  as  large  as 
desired.  This  assumption  is  not  unreasonable  for  continuous  motion  of 
the  robot  end  effector  since  the  end  effector  trajectory  is  a 
composition  of  the  joint  trajectories  (applying  the  well  known  theorem, 
the  composition  of  continuous  functions  is  continuous  and  vice  versa). 
The  acceleration  jump  will  be  estimated  in  terms  of  the  derivatives  of 
the  continuous  nth  order  differentiable  joint  trajectory. 

We  initially  estimate  the  acceleration  jump  for  an  on-line  splines 
joint  trajectory  with  no  velocity  correction  at  the  end  points  of  each 
computational  block.  For  a  2-point  look-ahead  (referring  to  Figure  2.4 
again  for  the  example  trajectory),  the  velocity  at  the  block  end  point 
(P3)  is  simply  the  derivative  of  the  quadratic  polynomial  between  P2  - 

P3- 

Let  ^-j.i'  ^-j »  ^-j+i  3nd  9^.^^,  9.^2»  ^•,•+3  denote  2  successive 
computation  blocks.  We  will  estimate  the  jump  in  acceleration  at  the 
(i+l)st  knot.    The  velocities    9.  ,  and  9.  „  are 
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Vl  =  (6.^^  -  6.)  -  e.  (2.74) 

The  acceleration  jump  at  the  (i+l)st  knot  is 
[Aa].,^  =  (2/t2)  [.  3  (9.^2  -  ^)  +  T  (9.  +  49.^^  ^1.^^)1 

=  (2/1^)  [-  3  (9.^2  -  9^)  +  T  {9.  +  (8/T)  (9.^^  .  9. )  -  49.  + 
^(V3-  V2)/3T[-  (V2^  Vl)/T-^n  (2.76) 
Rearranging  terms  in  Eqn.  (2.76), 
[Aa].^l  =  (2/1^)  [(1/3)  [9.^3  -  9.)  -  (7/3)  (9.^2  "  ^)  ^ 

(19/3)  (9.^^  -  9.)  -  (8/3)  9.]  (2.77) 
Expanding  the  terms  in  Eqn.  (2.77)  by  Taylor  series  about  9^  we  get 
9^  (3T)  +  9}^  (3T)2/2  +  9|11  (3T)3/6  + 
9|^^^  (3T)^/24  +  ...  (2.78) 
9J  (2T)  +         (2T)2/2  +  9JII  (2T)3/6  + 
gj^'^)  (2T)^/24  +  ...  (2.79) 


(V3  -  ^) 


(V2  -  ^)  = 
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(e.^j  -  e.)  =  ej  (t)  +  ej^  t^/2  +  ej^^  t^/3  +  ej^^^        +  ...  (2.80) 

From  Eqns.  (2.78),  (2.79),  (2.80),  and  Eqn.  (2.77) 

[Aa].^^  =  (2/T^)  [-  0.5555  T"^  ej^^  -  0.16667  e|^^^       -  0(T^)] 

=  -  [(1.111  T)  ej^^  +  (0.3334  T^)  e|^^)  +  0(T-^)]  (2.81) 

We  now  estimate  the  acceleration  jump  due  to  velocity  correction  at 
the  block  end  points  by  Eqn.  (2.59)  which  assumes  a  1-point  overlap. 
The  velocities  e^^^  and  e^.^^  ^O"^  this  case  (again  assuming  a  2-point 
look-ahead)  are  given  as 

e^+i  =  (1/3)  [(e.^2  ^  +  3  (6.^^  -  e.)/T  -  e.]  (2.82) 

Vz  =  (1/3)  [(6.^3  -  9.^2)/T  -  3  (e.^^  "  Vl^/^  -  Vl^  ^''^^^ 
Substituting  Eqns.  (2.82)  and  (2.83)  in  Eqn.  (2.73)  and  simplifying 
[Aa].^^  =  (2/T^)  [(0.^3  -  e.)/3  -  10  (e.^^  "  + 

13  (e.^j  -  e.)/9  -  2T  e./9]  (2.84) 

Expanding  the  terms  in  Eqn.  (2.84)  by  Taylor  series  as  before  and 
simplifying 

[Aa].^^  =  (2/T^)  [(0.2592  T^)  +  (0.444  T^)  eP^^  +  0(T^)]  (2.85) 
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[Aa].^^  =  (0.5185  T)  +  (0.8888  T^)  ej^^^  +  O(h^)  (2.86) 

Finally,  we  estimate  the  acceleration  jump  for  velocity  correction 
by  a  2-point  overlap  as  in  Eqn.  (2.67).  Note  that  in  this  case,  there 
is  no  acceleration  jump  at  the  block  end  point.  The  jump  in 
acceleration  is  at  the  point  preceding  the  block  end  point.  Therefore, 
with    the    two    blocks      6._^,  e. ,  e.^^  and  e.^^,  e.^^'  >  the 

accelerations  jump  is  at  the  ith  knot.    The  velocities    9.  and  6.^^  are 

e.  =  (1/T)  [(e.^j  -  9.)/3+  (6.  -  0._^)]  -  e._^/3  (2.87) 

e.^^  =  (1/llT)  [-  9.^3.  7  9.^2^  2  9.^^  -  9.  .  3  9._^]  .  9._^/ll 

(2.89) 


[Aa].  =  (2/T^)  [-  3  (9.^^  -  9._^)  +  (9._j  +49.+  9.^^)]  (2.90) 


[Aa].  =  (2/T^)  [-(9.^3  -  9._^)/ll  +  7  (9.^2  "  ^i-l^/H  " 


49  (9.^^  -  9._^)/33+  5  (9.  -  9._^)/3  -  8  T  9._^/33]  (2.91) 
[Aa].  =  (0.3838  T)  ^  (^-^^^  °  (T^)  (2. 92) 


Thus,  Eqns.  (2.81),  (2.86),  and  (2.92)  give  the  equations  for  the 
acceleration  jumps  due  to  on-line  splines  employing  no  velocity 
correction,  and  velocity  correction  with  a  1-point  and  2-point  overlap 
respectively  at  the  block  end  point.  To  a  first  degree  of  approximation 
on  T,  the  magnitude  of  the  acceleration  jumps  due  to  the  overlap  schemes 
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are  always  smaller  than  the  acceleration  jump  due  to  on-line  splines 
with  no  velocity  correction  at  the  block  end  point.  As  "T"  increases, 
the  second  order  terms  show  that  the  increase  in  the  magnitude  of  the 
acceleration  jump  is  more  rapid  in  the  splines  with  the  overlap 
correction  (Eqns.  (2.86)  and  (2.92)). 

Results  of  Simulation  and  Experimentation 

In  this  section,  the  performance  evaluation  by  simulation  and 
experimentation  of  Schemes  1  and  2  of  the  previous  section,  is 
presented. 

On-line  cubic  splines  approximation  of  joint  trajectories  provides 
the  servo  with  a  smooth  joint  displacement  and  velocity.  It  also 
ensures  better  tracking  of  the  end  effector  trajectory.  The  tracking 
accuracy  of  the  splines  is  shown  for  a  particular  Cartesian  move  by  the 
plots  (Fig.  2.8)  of  the  end  effector  path  error  in  mils  (1/1000  of  an 
inch)  versus  path  velocity  for  3  different  transform  update  rates.  The 
transform  update  rate  in  milliseconds  is  the  time  interval  between 
successive  joint  coordinates.  Experimental  plots  for  a  variety  of  robot 
moves  not  involving  singularities  and  limit  points  demonstrate 
characteristics  similar  to  those  in  Figure  2.8.  The  length  of  the 
linear  end  effector  move  used  in  our  example  simulation  was  1  meter. 
The  average  path  deviation  for  the  entire  path  for  different  velocities 
and  transform  update  rates  are  plotted  in  Fig.  2.8  for  this  move.  For 
comparison,  Figure  2.8  shows  the  average  path  error  for  a  path  velocity 
and  a  transform  update  rate  when  a  linear  function  is  used  for  joint 
trajectory  interpolation  (as  in  Figure  2.3)  and  cubic  splines  of  Scheme 
2  are  used  for  interpolating  the  same  joint  trajectory.     From  Figure 
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Figure  2.8      Maximum  path  deviation  vs.  path  velocity 
and  update  rates  for  linear  and  cubic  sp- 
lines interpolation  of  a  typical  Cartesian 
move 
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2.8,  it  is  seen  that  for  all  transform  update  rates  for  this  particular 
motion,  the  cubic  splines  track  the  end  effector  trajectory  with  greater 
accuracy  than  the  linear  interpolation  of  joint  coordinates. 

The  splines  cause  a  small  overshoot  at  points  on  the  joint 
trajectory  where  a  change  in  direction  occurs.  Experimentation  with  a 
large  number  of  linear  end  effector  moves  has  shown  that  the  error  in 
end  effector  trajectory  due  to  a  small  overshoot  in  one  or  more  joint 
angles  is  insignificant,  unless  operating  close  to  a  joint  limit. 
Figure  2.9  shows  a  joint  trajectory  which  is  almost  linear  with  time. 
Cubic  splines  of  Scheme  2  with  a  transform  update  rate  of  100  ms  are 
used  for  interpolating  the  joint  trajectory.  Notice  that  the  cubic 
splines  track  this  trajectory  with  no  overshoot. 

Both  Schemes  1  and  2  were  tested  on  a  6  degrees  of  freedom  robot, 
the  kinematic  structure  of  which  is  shown  in  Figure  2.2. 
Experimentation  with  a  range  of  transform  update  rates  from  50ms  to 
200ms  showed  that  the  cubic  splines  produced  smooth  motion  of  the  joints 
during  a  Cartesian  motion  of  the  end  effector.  Linear  interpolation  of 
joint  coordinates  showed  visible  jerkiness  in  motion  for  update  rates 
higher  than  100ms. 
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CHAPTER  III 

ROTATIONAL  SPACE  CURVES  FOR  CONSTRUCTION  AND  COMPLETE 
DESCRIPTION  OF  ROBOT  TOOL  ORIENTATION  TRAJECTORIES 

In  Chapter  II,  we  developed  novel  techniques  for  the  construction 
of  joint  space  trajectories  for  both  joint  interpolated  motion  and 
constrained  tool  motion  of  the  robot  manipulator.  In  order  to 
accurately  track  the  end  effector  path  by  the  on-line  approximation  of 
the  joint  trajectories,  the  controlled  end  effector  trajectory  in 
cartesian  space  must  conform  to  the  "feasibility  constraints"  of  the 
manipulator.  Feasibility  constraints  on  the  tool  motion  are  the  maximum 
tool  velocity  and  acceleration  given  by  the  physical  bounds  on  the 
velocity  and  acceleration  of  the  individual  joints.  Since  the  tool  is 
translating  as  well  as  rotating  about  an  axis  in  space,  both 
translational  and  rotational  motions  of  the  tool  must  satisfy  the 
feasibility  constraints.  The  current  approach  to  planning  an  end 
effector  motion  is  "translation-dominant"  [Paul  1981,  Taylor  1979].  A 
translation  dominant  approach  is  the  planning  of  a  trajectory  based  on 
the  translational  move  parameters.  The  rotational  trajectory  is 
constructed  from  the  move  time  of  the  translational  trajectory.  For 
those  motions  of  the  end  effector  where  there  is  a  greater  rotation  than 
translation,  as  in  the  rotation  of  the  tool  in  the  tool  frame, 
translation  dominant  techniques  may  be  deficient  or  even  fail  to  produce 
a  continuous  and  "feasible"  rotational  motion  of  the  manipulator. 
Again,  by  "feasible"  rotational  motion,  it  is  meant  that  the  rate  of 
change  of  the  tool    rotation  and  the  acceleration  of  the  change  is 
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limited  by  the  maximum  rate  and  acceleration  constraints  on  the  joints 
of  the  manipulator.  Also,  a  translation-dominant  move  makes  the 
rotational  error  in  moves  such  as  those  requiring  an  instantaneous  turn 
of  the  robot  tool,  proportional  to  the  translational  move  parameters. 
Since  translation  and  rotation  are  independent  motions,  we  introduce, 
develop,  and  describe  the  techniques  for  the  independent  determination 
of  the  tool  rotational  trajectories  in  this  Chapter. 

End  Effector  Motion  Specification  and  Trajectory  Planning 

It  is  common  in  practice  to  specify  the  translational  trajectory  of 
a  robot  tool -tip  to  the  required  degree  of  exactness  for  a  given  robot 
task  in  3-space.  Since  the  tool,  in  general,  also  rotates  along  its 
translational  space  curve,  and  rotation  and  translation  are  independent 
motions,  a  complete  specification  of  the  rotational  trajectory  is  in 
order.  Emphasis  on  rotational  trajectories  has  been  underplayed  in 
robot  motion  since  it  is  difficult  to  visualize  and  specify  an  exact 
tool  orientation  trajectory.  Therefore,  as  long  as  the  rotation  is 
uniform  and  smooth  along  the  translational  tool  trajectory,  no 
constraints  are  imposed  on  the  specific  nature  of  the  orientation 
trajectory. 

The  simplest  tool-tip  trajectories  in  Cartesian  space  are  either 
linear  or  a  composition  of  linear  paths.  Along  a  point  to  point  linear 
path,  there  are  two  well-known  methods  of  rotation  interpolation.  One 
approach  is  to  determine  an  axis  in  space  such  that  the  tool  rotates 
about  this  axis  with  constant  velocity  and  the  total  rotation  about  this 
axis  transforms  the  initial  tool  frame  to  the  final  tool  frame  [Taylor 
1979].     A  second  method  described  in  [Paul   1979]  uses  the  freedom  of 


66 

choice  of  the  rotational  space  curve  to  split  the  total  rotation  about 
the  linear  path  into  2  independent  rotations.  One  rotation  aligns  the 
tool  vector  from  an  initial  to  the  final  orientation,  and  the  other 
rotation  is  about  the  tool  axis  itself,  to  align  the  faceplate 
vectors.  The  two  rotations  are  interpolated  linearly  between  the  start 
and  destination  points.  The  intermediate  orientations  of  the  tool  on 
the  linear  path  for  Paul's  and  Taylor's  schemes  are  generally  dif- 
ferent. The  non-existence  of  a  unique  rotational  trajectory  illustrates 
the  fact  that  the  rotational  motion  of  the  tool -tip  on  a  cartesian  path 
is  underspecified.  One  of  the  objectives  of  this  Chapter  is  to  define 
additional  constraints  on  the  rotational  space  curve  to  resolve  some 
ambiguity  in  interpolating  the  tool  orientation  along  any  Cartesian 
space  curve  of  the  robot  tool.  Two  approaches  are  described  for  the 
specification  of  constraints  on  the  rotational  space  curve.  The  first 
one  is  based  on  a  description  of  the  relationship  between  the  rotation 
space  curve  and  the  translation  space  curve.  The  second  approach  is  to 
completely  specify  the  rotational  curve  in  rotation  space  only. 

For  most  point  to  point  moves,  a  uniform  tool  rotation  is  desirable 
due  to  smaller  inertial  forces  on  the  tool.  If  the  objective  is  to 
simply  transform  the  tool  smoothly  from  one  orientation  to  another,  as 
in  a  tool  transiting  between  work  points,  or  tracking  an  object  based  on 
sensory  feedback,  the  only  constraint  on  the  tool  motion  is 
smoothness.  We  term  this  type  of  tool  motion  unconstrained.  Another 
example  of  unconstrained  tool  rotation  is  during  the  transition  of  the 
tool  translational  path  from  one  linear  segment  to  another.  The  only 
constraints  on  the  tool  rotational  trajectory  in  a  motion  involving  a 
transition   from  one  linear  segment  to  another   is   the  continuity  in 
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position  and  velocity  at  the  2  boundary  points.  Most  of  the  existing 
techniques  for  a  smooth  unconstrained  tool  rotation  employ  a 
"translation  dominant"  approach  for  computing  the  time  for  the  tool 
rotational  transition.  The  translation  dominant  approach  is  to  compute 
the  move  time  based  on  the  translational  parameters  (linear  velocity  and 
acceleration)  of  the  tool.  For  some  robot  moves  where  the  translational 
displacement  is  much  smaller  than  the  rotational  displacement  of  the 
robot  tool,  a  translation  dominant  approach  may  cause  excessive 
acceleration  demands  on  some  joint  servos  of  the  manipulator.  In  this 
Chapter,  we  separate  the  translational  and  rotational  motion  by 
decoupling  the  manipulator  degrees  of  freedom.  A  common  wrist  for  many 
6  degrees  of  freedom  (DOF)  industrial  manipulators  is  the  spherical 
wrist.  In  a  spherical  wrist,  the  3  axes  of  the  degrees  of  freedom  from 
the  wrist  to  the  tool  intersect  at  the  wrist  joint.  For  manipulators 
with  spherical  wrists,  we  derive  a  simple  method  for  the  independent 
determination  of  the  rotational  move  parameters  by  decoupling  the 
manipulator  DOFs  at  the  wrist  into  translational  and  rotational  DOFs. 

There  are  some  robot  tasks  that  require  constrained  changes  of  the 
robot  tool.  For  example,  in  an  arc  welding  task,  the  orientation  of  the 
torch  with  respect  to  the  work  surface  is  always  of  primary  concern. 
Thus,  in  case  of  an  orientation  change,  the  torch  angle  with  respect  to 
the  work  must  be  maintained  constant.  This  type  of  orientation  change 
is  termed  constrained.  Motions  with  constrained  tool  orientation  are 
executed  by  transforming  the  constraints  into  joint  coordinates  at  every 
sample  time  instant.  A  simple  method  of  determining  the  maximum 
constraints  (velocity  and  acceleration)  on  the  constrained  rotational 
motion  is  given  in  later  sections. 
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In  order  to  construct  a  continuous  rotational  space  curve  for  an 
off-line  trajectory,  a  3-dimensional  "rotation  space"  is  defined.  A 
rotation  space  is  defined  by  rotations  about  three  orthogonal  axes  of  a 
fixed  frame,  such  that  a  point  in  the  frame  represents  a  tool 
orientation.  The  order  of  rotations  about  the  axes  of  the  rotation 
space  is  fixed.  Therefore,  either  the  roll,  pitch  and  yaw  angles  or  the 
three  Euler  angles  can  be  used  for  creating  the  rotation  space  with 
respect  to  the  fixed  frame.  For  most  articulated  6-degrees  of  freedom 
robot  manipulators,  the  first  three  joints  determine  the  wrist  position 
and  the  last  three  joints  the  tool  orientation  from  the  wrist.  There 
are  different  types  of  wrist  joints  in  articulated  manipulators.  We  can 
classify  the  wrist  joints  into  2  common  types,  an  Euler  wrist  (spherical 
wrist)  and  an  R-P-Y  (roll,  pitch,  yaw)  wrist.  The  choice  of  the 
specific  rotation  space  is  based  on  the  type  of  wrist  of  the 
manipulator. 

In  the  rotation  space,  well  known  curve  fitting  techniques  can  be 
employed  for  creating  a  smooth  rotational  space  curve  through  a  number 
of  via  points.  Also,  transition  from  one  rotational  space  curve  to 
another  in  rotation  space  can  be  conveniently  smoothed  to  any  degree  by 
employing  polynomial  smoothing  techniques.  A  continuous  (at  least  in 
the  first  time  derivative)  rotation  space  curve  not  only  gives  a  unique 
rotational  trajectory  for  a  move,  but  also  guarantees  a  smooth 
transition  from  one  velocity  to  another  without  having  to  stop  at  the 
point  of  transition. 

'  The  next  section  describes  the  construction  of  rotation  space. 
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Definition  of  a  Rotation  Space 


A  continuous  rotation  space  can  be  created  by  choosing  3 
independent  rotations  of  the  tool  about  a  fixed  base  or  universal 
frame.  Thus,  roll,  pitch  and  yaw;  or  the  3  Euler  angles  can  be  employed 
to  create  the  rotation  space.  The  basis  of  the  rotation  space  will  be  3 
orthonormal  axes,  each  axis  representing  the  roll,  pitch  or  yaw,  or  the 
3  Euler  angles  about  the  axes  of  a  pre-defined  base  or  universal 
frame.  Any  orientation  of  the  robot  tool  maps  to  a  point  in  the 
rotation  space.  The  space  curve  of  the  points  in  rotation  space  for  a 
given  move  is  called  the  rotation  space  curve. 

We  use  standard  definitions  of  roll,  pitch  and  yaw  or  the  Euler 
angles.  The  roll  is  defined  as  the  rotation  about  the  z-axis  of  the 
base  frame,  the  pitch  as  rotation  about  the  y-axis  and  the  yaw  as  the 
rotation  about  the  x-axis.  The  rotation  matrix  transforming  the  base 
frame  to  the  tool  frame  for  a  R-P-Y  point  (  <|),  6,  t  )  in  rotation  space 
can  be  derived  [Paul  1981]  as 


b, 


R^  =  Rot  ((z,.).),  (y,9),  (x,i|»)) 


cos  (|)  COS  9 

sin  (t)  cos  8 
-  sin  9 


cos  <t)  sin  9  sin     -  sin  (j)  cos  \p 
sin  (}i  sin  6  sin  ^+  cos  <})  cos  ^ 
cos  8  sin  i|) 


cos  <{>  sin  9  cos  ^+  sin  <^  sin  ^ 
sin  (|)  sin  9  cos  t  -  cos  (j)  sin  \p 
cos  9  cos 


(3.1) 


Similarly,   the   transformation  of  an   Euler  point     (({i,  9,  ^p)  is 
given  by  the  following  transformation  matrix: 
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•^R^  =  Rot  {z,(t))  Rot  (y,9)  Rot  (z,t) 


cos  <\>  cos  9  COS  ^  -  sin  (j)  sin  \|) 
sin  (j)  cos  e  cos  ^  +  cos  ^  sin  \|) 
-  sin  9  cos  ij) 


-  cos  <|)  cos  9  sin  ij)  -  sin  (|)  cos 

-  sin  (j)  cos  9  sin  i|)  +  cos  <}>  cos 

sin  9  sin  \p 


cos  (j)  sin  9 
sin  (j)  sin  9 
cos  9 


(3.2) 


The  matrix  R^  is  a  transformation  relating  the  base  and  tool 
frames.  In  the  following  sections,  methods  for  unconstrained  and 
constrained  rotational  transitions  are  described.  The  main  emphasis  is 
on  separating  the  determination  of  the  move  parameters  for  translational 
and  rotational  parts  of  a  motion. 


All  rotational  transitions  can  be  classified  into  constrained  and 
unconstrained  rotations  of  the  manipulator  tool.  In  this  section,  we 
classify  the  constraints  on  a  rotational  trajectory  into  two  types  of 
constraints  called  explicit  and  implicit  constraints. 

Let  us  consider  the  simplest  end  effector  move  in  Cartesian  space, 
a  point  to  point  linear  move.  A  point  to  point  linear  move  is  defined 
by  specifying  the  position  and  orientation  (referred  to  as  pose)  of  the 
manipulator  at  2  points  in  the  work  space  of  the  robot.  The  translation 
interpolation  is  straight  forward  since  the  desired  trajectory  (linear) 
is  specified.  However,  the  rotational  space  curve  has  no  constraints  in 
the  move  specification.  Constraints  on  the  rotational  space  curve  can 
be    specified    in    relation    to    the    translational     space    curve  or 
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independently.  A  description  of  the  constraints  to  be  specified  for  a 
well-defined  rotational  space  curve  are  classified  into  explicit 
constraints  and  implicit  constraints.  Explicit  constraints  directly 
specify  the  nature  of  the  rotation  space  curve  by  giving  a  number  of 
intermediate  target  points.  Implicit  constraints  specify  the  rotational 
trajectory  as  a  function  of  the  translational  trajectory. 

Explicit  Constraints  on  the  Rotational  Trajectory 

Let  us  assume  that  on  the  linear  path,  the  tool  orientation  is 
given  at  one  or  more  intermediate  points.  The  larger  the  number  of 
points  in  rotation  space  for  a  move,  the  better  the  approximation  to  the 
rotational  space  curve  (in  general).  This  form  of  constraint 
specification  is  termed  explicit,  since  interpolation  points  are 
directly  available  in  the  rotation  space  and  the  rotational  space  curve 
can  be  approximated  by  a  polynomial  interpolation  through  the 
intermediate  points. 

Well-known  polynomial  interpolation  techniques  can  be' employed  for 
constructing  the  rotational  space  curve  from  a  number  of  via  points  in 
rotation  space.  If  only  2  points  (start  point  and  destination  point)  in 
rotation  space  for  a  point  to  point  linear  move  are  given,  then  one 
solution  would  be  to  approximate  the  rotational  trajectory  by  a  quintic 
polynomial,  since  a  total  of  6  constraints  (position,  velocity  and 
acceleration)  are  known  at  the  2  end  points.  For  the  quintic 
approximation  in  rotation  space,  the  direction  and  magnitude  of  the  tool 
rotational  velocity  will  not  be  constant,  and  the  intermediate  tool 
orientations  with  respect  to  the  translational  curve  will  be 
unpredictable. 
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If  the  rotational  motion  is  to  be  at  a  constant  angular  velocity, 
then  appropriate  via  points  (for  interpolation)  in  the  rotation  space 
must  be  specified  in  order  to  generate  the  correct  space  curve. 

Implicit  Constraints  on  the  Rotational  Trajectory 

Implicit  constraints  define  the  rotational  motion  with  respect  to 
the  translational  tool  tip  trajectory.  For  example,  one  can  specify  the 
tool  vector  always  be  along  the  normal  to  the  tool  tip  trajectory.  A 
typical  application  where  the  robot  tool  tip  may  be  specified  along  the 
normal  to  the  translational  space  curve,  is  tool-tip  motion  along  a 
circular  arc.  If  the  rotation  about  the  tool  axis  is  given  as  uniform, 
then  the  robot  tool  traces  the  circular  arc  with  the  tool  orientation 
always  along  the  normal  and  uniform  rotation  about  the  tool  vector. 
Implicit  constraints  must  be  transformed  to  generate  intermediate  points 
in  the  rotation  space  such  that  a  rotation  space  curve  can  be  generated. 

Another  implicit  constraint  on  tool  vector  orientation  can  be 
specified  with  respect  to  the  initial  and  final  tool  vector 
orientations.  The  constraint  could  be  that  the  intermediate  tool 
orientations  must  lie  in  the  plane  formed  by  the  initial  and  final  tool 
vectors  (both  vectors  expressed  as  unit  vectors  at  the  origin  of  the 
tool  frame  at  the  start  point).  This  constraint  is  used  in  Paul's 
scheme  for  generating  the  tool  rotation  [Paul  1979,  1981].  An  advantage 
of  imposing  the  "coplanarity  constraint"  on  intermediate  tool  vector 
orientations  is  illustrated  by  an  example.  Assume  that  a  linear  move  of 
the  robot  end  effector  is  programmed  such  that  the  initial  and  final 
tool  vectors  are  coplanar.  If  the  equivalent  axis  of  rotation  approach 
[Taylor  1979]  is  used  to  interpolate  rotations,  then  there  is  no 
guarantee  that  the  tool  orientation  will  lie  in  the  plane  of  the  initial 
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and  final  tool  vectors.  Thus,  the  constraint  that  the  tool  always  lie 
in  the  plane  formed  by  the  initial  and  final  tool  vectors  restricts  the 
freedom  of  the  rotational  space  curve.  There  is,  however,  the  choice  of 
the  curve  within  the  plane  of  the  2  tool  vectors  for  the  intermediate 
orientations.  If  the  rotations  are  to  be  uniform,  then  this  curve  will 
be  linear. 

Any  constraints  on  the  tool  vector  in  Cartesian  space  have  to  be 
transformed  into  rotation  space  as  interpolation  points.  Thus,  for  a 
point  to  point  move,  if  the  tool  vector  is  constrained  to  a  certain 
trajectory,  each  sample  point  on  this  trajectory  has  to  be  transformed 
into  the  rotation  space  angles.  Polynomial  interpolation  through  these 
sample  points  for  the  angles  in  rotation  space  yields  the  rotation  space 
trajectory  for  constrained  tool  motion.  For  an  off-line  creation  of  a 
rotational  trajectory,  interpolation  points  must  be  determined  in  the 
rotational  space  based  on  a  technique  like  the  bounded  deviation  joint 
paths  method  [Taylor  1979]  that  minimizes  the  rotation  error  to  a 
certain  specified  value. 

In  the  next  section,  we  describe  a  conventional  translation- 
dominant  approach  for  planning  rotational  transitions.  Since 
translation  and  rotation  are  independent  motions,  we  derive  a  method  for 
planning  independent  rotation  transitions  in  the  following  sections.  In 
order  to  decouple  the  rotational  motion  from  the  translation,  we  need  an 
estimate  of  the  rotational  tool  acceleration  in  the  robot  workspace. 
Equations  for  the  rotational  acceleration  are  developed  for  a  special 
class  of  manipulators  with  spherical  wrists. 
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Construction  of  Rotational  Trajectories  --  Conventional 
Translation  Dominant  Approach 

In  this  section,  we  describe  some  of  the  disadvantages  of  a 
translation  dominant  approach  for  computing  the  rotational  move.  An 
example  move  is  chosen  to  demonstrate  the  effect  of  planning  a  rotation 
trajectory  based  on  the  transl ational  trajectory  parameters.  The 
example  move  is  a  transition  of  the  robot  tool  from  one  constant 
velocity  linear  segment  to  another.  In  this  case,  the  points  for 
starting  and  ending  the  transition  must  be  determined.  The  translation 
dominant  approach  employs  the  constraint  on  the  maximum  linear 
acceleration  of  the  tool  to  produce  a  symmetric  transition  of  the  tool- 
tip. The  same  time  period  is  used  for  the  rotational  change  also.  It 
is  shown  that  the  rotational  error,  defined  as  the  rotational  deviation 
from  an  intermediate  point  that  the  tool  misses  due  to  the  continuous 
transition,  is  directly  proportional  to  the  transition  time.  Thus,  if 
the  translation  transition  time  is  high,  the  rotational  error  will  be 
large,  although  the  rotational  change  may  be  small  between  the  points. 

In  Figure  3.1,  let  PI  be  the  point  where  the  robot  starts  from 
rest,  moves  linearly  with  a  constant  velocity  v^  to  point  P2,  turns 
instantaneously  at  P2,  moving  linearly  with  a  constant  velocity  V2  to 
point  P3.  From  the  tool  orientations  at  PI  and  P2,  an  axis  of 
rotation,  k_  ^  for  rotating  the  initial  tool  frame  at  PI  uniformly  into 
the  frame  at  P2  is  determined  [Taylor  1979]  (see  [Paul  1981]  for 
derivation  of  k_  ^  ).  Another  axis  of  rotation,  k.  2  »  ''^  determined  for 
rotating  the  frame  at  P2  into  the  frame  at  P3.  The  move  specification 
requires  that  the  robot  tool  turn  instantaneously  at  P2  without 
stopping.     Since  this  cannot  be  done  physically,  such  motion  must  be 
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Figure  3.1 


A  segment  to  segment  transition 
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approximated.  An  accepted  method  of  turning  the  tool  at  P2  is  to 
execute  a  "flyby."  A  flyby  begins  turning  the  tool  before  P2  such  that 
the  tool  misses  the  point  P2  but  makes  a  smooth  transition  to  the  new 
segment  P2-P3  [Paul  1979,  Taylor  1979],  The  degree  of  the  polynomial 
for  the  segment  to  segment  transition  of  the  tool  tip  is  reduced  by 
making  the  transition  curve  symmetric  with  constant  linear 
acceleration.  The  time  for  the  transition  is  determined  from  the  two 
linear  velocities,  v_  ^  and  _v  2»  and  the  maximum  linear  tool 
acceleration,  a     ,  as  follows: 


where  2t  is  the  time  for  the  transition. 

Regardless  of  the  orientation  change  required  between  the  two 
segments,  the  transition  time  2t  determined  from  the  transl ational 
motion  parameters  is  also  employed  for  the  rotational  transition.  The 
main  advantage  of  using  the  transl ational  transition  time  for  the 
rotational  change  is  the  simplicity  in  executing  the  motion,  since  the 
translational  and  rotational  transitions  begin  at  the  same  time  and  end 
at  the  same  time.  The  tool  orientation  is  required  to  change  from  the 
axis  of  rotation  k_  ^  and  velocity  ^  ,  to  the  axis  of  rotation  k  ^  and 
velocity  u  ^  •  The  following  equation  produces  a  smooth  transition 
(unconstrained),  keeping  the  orientation  and  rotational  velocity 
continuous  at  both  ends  of  the  transition. 


2t  =  I  V  J  -  V  2  |/a 


max 


(3.3) 


R(t)  =  R^.  Rot  (k_ 


(t  -  f)^ 


ojj  .  Rot  (k 


0^2)  (3.4) 


4t 


^  2' 
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where  t'  =  t  -  T^,  being  the  move  time  for  the  segment  P1-P2  and  R2 
is  the  rotation  matrix  at  the  "flyby  point"  P2.  The  rotation  matrix 
R(t)  after  t  seconds  into  the  flyby  segment  (t  =  T^)  is  given  by 


R(Tj)  =       .  Rot  (l<_ 


a)^T/4)  .  Rot  (k_  2  5  '^2'^^^^ 


(3.5) 


If  the  robot  end  effector  were  to  actually  pass  through  the 
intermediate  point  P2,  the  rotation  matrix  R(t)  at  t  =  T-^  seconds  should 
be  R2,  the  orientation  at  the  point  P2.  If  (  £  p  £  p  a.  1^)  denote  the 
3  column  vectors  of  R(T^)  (Eqn.  (3.5)),  and  (n^,  0^,  a_)  denotes  the 
column  vectors  of  the  actual  rotation  matrix  at  P2,  then  the 
orientation  error  is  defined  as  (see  [Luh  1980b]  for  derivation) 


1=2    iijl  ^  a  i)      (£X£^)  +  (a_xa_^)] 


(3.6) 


In  order  to  estimate  the  rotation  error  ci  as  a  function  of  t  for 
Eqn.  (3.5),  let  Ae^  =  -  u^x/A,  and  AS^  =  oj^t/A.  For  small  error 
analysis,  we  assume  that    Ae^  and    A92  are  small.    Expanding  Eqn.  (3.5), 


R(T^) 


=  R, 


^1  ^^1 


k  .  A9- 
xl  1 


k  ,  Ae, 

yi  1 

-k  1  Ae^ 

xl  1 


^2  ^^2 
•^2  ^^2 


-^2  ''2 


^2  ^^2 
-^2  ^^2 
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1  -k^i  Ae^  -       Ae^  Ae^  .  k^^  Ae^ 

k^^  A9^  .  k^2  ^^2  1  -^2  ^^2  -^1  '\ 

•^1  ^^1  -  ^2  ^^2       ^1         '  \2  ''2  1 


(3.7) 


All  second  order  terms  in    Ae  are  ignored  in  Eqn.  (3.7).  Let 


^1  =  ^1        '  ^2  ^^2 


(3.8) 


'2  -  ^1  "'l      S2  "^2 


(3.9) 


^3  =  ^1  '\  '  \2  ''2 


(3.10) 


Eqn.  (3.7)  can  be  written  as 


1  -e 

=  R2 

^1  ' 
.  "2  ' 

Let 

\  =  [n..  £. 

_a].  Substituting 

n   +  e,  0 

X          1  X 

■  ^2  \ 

-n    e,  + 

X  1 

Rd^)  = 

"y  '  °y 

"z  ^  ^1  °z 

-  ^2  ^ 

-"z  ^1  ^ 

(3.11) 


D   +  a    e^  n  e„-  o    e,  +  a 

X  x3       x2x3  X 

V  '  'y  '3  °y  ^3  ' 

\  ^  ^3  "z  ^2-  °z^  3  ^  ^z 


(3.12) 
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The     rotational     error  a  is     now     derived     from     Eqn.  (3.6). 
(n_  ^  ,  £  ^  ,  a_  J  )  are  the  3  columns  of  Eqn.  (3.12).    Taking  the  cross 
products  and  simplifying  the  terms  we  get 

a   =    [{e^.  (n_  X  o_)^  +  e^.  (a^  x  n_)^  +  e^.  (o_  x  a_)^}  j_ 
+  {e^.  (n_  X  o_)y  +  e^.  (a_  x  n_)y  +  e^.  (o  x  a^)^}  j_ 

+  {e^.  (n_  X  o_)^  +  e^.  (a_  x  n) ^  +  e^.  (o_  x  a_)^}  k_] 

=    [e^  a_+  62  £  +  63  n]  (3.13) 


a  .  a  =  I  a  |2  =  (e^^  +  e^^  +  e3^)  (3.14) 


Substituting  A9^  =  -  t/4  and  ^Q^""  '^2  '^^^  ^(]r]s.  (3.8) 
(3.10), 


=  (t/4)  (-k^^  0)^  +  k^2  '"z^  ^^-^^^ 


62  =  (t/4)  (-k^j  0)^  +  ky2  "^2^  (^•^'^^ 


63  =  (t/4)  (-k^^  (0^  +  k^2  '"2^  ^^-^^^ 
Substituting  Eqns.  (3.15)  -  (3.17)  in  Eqn.  (3.14), 


|a|^  =  (t^/16)   (o)^  -  2  03^  0)  2    i  1*    Ji  2      "^2  ^ 


|a|  =  (t/4)  .  (u)^  -  2a)j  0)2  I<  j  .  I<  2  +  4  18) 
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Therefore,  the  magnitude  of  the  rotational  error  is  directly 
proportional  to  the  transl ational  flyby  time  x,  independent  of  the  arm 
kinematic  structure.  A  plot  of  |_a|  versus  t  in  Figure  3.2  illustrates 
the  linear  relationship  between  rotational  error  and  x.  The  kinematic 
structure  of  the  robot  used  for  generating  the  plot  of  Figure  3.2  is 
shown  in  Figure  2.2.  The  move  parameters  for  the  plot  are  given  in 
Table  3.1.  A  maximum  linear  tool  acceleration  of  1  m/sec^  was  employed 
for  the  determination  of  flyby  time  "2x." 

Table  3.1    Move  parameters  for  Figure  3.2 


JOINT  COORDINATES  (DEG.)  LINEAR  VELOCITY 

^1       ^2       ^3       ^4       ^5  ^6 

Start  point  Pi        0        0        0        0        0        0  0  m/sec 


Intermediate  50        0        0       90       45        0        .07  m/sec 

point  P2 


End  point  P-5        100       45       -45       0       90        45       .15  m/sec 


Thus,  the  rotational  flyby  error  for  a  rotation  transition  given  by 
Eqn.  (3.4)  directly  depends  on  the  velocity  gradient  of  the  linear 
transl ational  path,  a  dependence  not  justified  by  the  independence  of 
the   transl ational    and    rotational    motions.       If    x    is  independently 
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determined  for  the  rotational  transition,  the  rotational  error  |a|  can 
be  made  smaller  and  also  independent  of  translational  move  parameters. 

In  order  to  determine  a  rotational  flyby  time,  we  need  an  estimate 
of  the  maximum  rotational  acceleration  of  the  robot  tool.  Since  the 
rotational  acceleration  of  the  tool  is  obviously  a  function  of  the  robot 
pose,  a  conservative  value  must  be  determined  such  that  it  is  valid  for 
the  entire  workspace  of  the  robot.  One  way  to  determine  the 
instantaneous  rotational  acceleration  of  the  tool  frame  is  to  express 
the  tool  frame  as  a  rotation  about  an  axis  in  space.  Let  (_k,  6)  denote 
this  axis  and  the  rotation  about  the  axis  respectively.  If  the  tool 
frame  vectors  are  denoted  as    (n,  o,  a)  ,  then  9  is  given  as 


e  =  tan"^    /(o^  -  aj^+  (a    -  n  )  ^+  (n    -  o  )  ^ 

 ^  ^   (3.19) 

n   +  0  +  a    -  1  ^  ' 

X      y  z 

T  T  T 

where        n_  =  (n^,  n^,  n^)    ,    £  =  (o^,  o^,  o^)    and  a_  =  (a^,  a^,  a^)  . 

Since     (n[,  £,  a_)  are   functions   of  the  joint  angles     (9^  ..  9g),  Eqn. 

(3.19)  can  be  written  as 


9  =  f  (9^  ..  9g)  .  (3.20) 


Differentiating  Eqn.  (3.20)  twice  with  respect  to  time,  and 
substituting  for  the  maximum  values  of  the  joint  rates  and 
accelerations,  we  can  obtain  a  value  for  the  instantaneous  tool 
acceleration  when  all  the  joints  are  driven  at  their  maximum  rates. 
Since  the  rotational  acceleration  is  a  function  of  the  arm  pose,  the 
magnitude  of  the  rotational  acceleration  varies  at  different  points  in 
the   robot   workspace.      The   equation   for  the   rotational  acceleration 
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obtained  by  differentiating  Eqn.  (3.20)  is  a  non-linear  function  of  the 
joint  angles,  velocities  and  accelerations  with  a  large  number  of 
coupling  terms  in  the  joint  parameters.  Therefore,  a  mathematical 
estimation  of  a  maximum  magnitude  of  the  rotational  acceleration  that  is 
usable  in  the  entire  robot  workspace  independent  of  the  arm  pose  and 
velocity,  is  non-trivial  by  this  approach. 

In  the  following  sections  of  the  chapter,  we  analytically  derive 
the  limits  on  the  tool  rotational  acceleration  by  decoupling  the 
rotation  into  two  rotations  about  the  robot  wrist. 

Construction  of  Independent  Rotational  Motions 

In  this  section  we  describe  methods  for  producing  independent  tool 
rotations.  In  order  to  determine  an  independent  move  time  for  the 
rotations,  we  need  an  estimate  of  the  maximum  rotational  acceleration  as 
mentioned  earlier.  For  certain  unconstrained  tool  rotations,  the  limits 
on  the  maximum  joint  rates  and  accelerations  can  be  directly  employed  to 
determine  a  rotational  move  time.  For  many  other  rotational  moves,  we 
need  an  estimate  of  the  tool  rotational  acceleration,  which  is  a 
composition  of  all  the  joint  rates  and  accelerations.  This  estimate  of 
the  tool  rotational  acceleration  must  be  valid  for  the  entire  robot 
workspace,  independent  of  the  arm  configuration.  We  derive  the 
equations  for  the  estimation  of  the  rotational  acceleration  of  the 
tool.  The  derivation  is  specifically  for  the  most  common  industrial 
articulated  robot  geometry  -  manipulators  with  spherical  wrists.  The 
spherical  wrist  decouples  at  the  "wrist"  joint  of  the  articulated 
manipulator  into  the  so  called  position  and  orientation  degrees  of 
freedom.    This  property  is  used  to  decouple  the  rotational  motion  into 
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rotation  due  to  the  orientation  angles,  and  rotation  due  the  positioning 
angles.  The  decoupling  is  employed  for  planning  independent  rotational 
motions  for  the  orientation  angles,  6^,  6^  and  Sg  .  An  estimate  of  the 
rotational  acceleration  in  the  "rotation  space"  formed  by  the 
orientation  angles  is  determined.  Due  to  the  simplification  afforded  by 
the  decoupling,  the  exact  nature  of  the  rotational  acceleration  in  the 
robot  workspace  is  easily  visualized  and  analytically  proven.  We 
completely  analyze  one  type  of  a  wrist,  namely  the  "Euler"  wrist,  for 
maximum  rotational  velocity  and  acceleration  of  the  tool.  The  results 
of  the  analysis  are  applied  to  a  simulation  example  that  computes 
independent  rotational  transitions.  The  plots  of  the  joint  angles 
versus  time  show  the  improvement  in  the  joint  trajectories  due  to  an 
independent  rotation  trajectory  computation  as  compared  to  a  translation 
dominant  approach  for  trajectory  determination. 

The    next    section    describes    the    estimation    of    a  rotational 
acceleration  of  the  tool  in  the  robot  "rotation  space." 

Planning  Constrained  Rotation  Transitions  -- 

Estimation  of  Acceleration  For  Spherical  Wrist  Manipulators 

For  a  manipulator  with  rotary  joints,  the  rotational  velocity  and 

rotational  acceleration  of  the  tool  is  limited  by  the  joint  velocities 

and  accelerations.    Table  2.3  shows  the  full-load  joint  velocities  and 

accelerations  of  the  manipulator  of  Figure  2.2.    The  waist  rotation  6^ 

has   the   smallest    full -load   acceleration    since    for   most   moves,  the 

loading  on  joint  1  is  the  maximum.    We  observe  from  Table  2.3  that  the 

full-load   velocities   and  accelerations   of  the   last  three  rotations, 

6^,  9g  and  9g,  are  approximately  twice  as  high  as  the  velocities  and 

accelerations  of  the  first  three  rotations,     9,,  9„  and  9-.     This  is 
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typical  of  articulated  robots  with  a  spherical  wrist.  In  this  section, 
we  decouple  the  manipulator  degrees  of  freedom  at  the  wrist  into 
positional  and  rotational  degrees  of  freedom.  The  decoupling  of  the 
translation  and  rotation  at  the  wrist  yields  a  less  conservative 
estimate  of  the  rotational  acceleration  as  compared  to  the  case  where 
the  decoupling  is  not  possible,  since  the  rotational  velocity  and 
acceleration  constraints  are  determined  independently  for  the  first  3 
and  the  last  3  degrees  of  freedom. 

Decoupling  rotational  motion  at  the  "wrist"  joint 

For  most  6  degrees  of  freedom  robot  manipulators,  the  wrist  joint 
(Figure  3.3)  can  be  regarded  as  the  decoupling  joint  between  3  position- 
ing degrees  of  freedom  and  the  3  orientation  degrees  of  freedom.  The 
first  3  joints  are  called  the  positioning  joints  and  the  last  3  joints 
the  orientation  joints.  The  decoupling  at  the  wrist  has  been  employed 
to  simplify  the  computations  in  the  reverse  (or  world  to  joint)  trans- 
formation, and  the  inverse  dynamics  for  the  robot  [Hollerbach  and  Sahar 
1983].  We  define  the  3  orientation  degrees  of  freedom  as  the  basis  for 
a  tool  "rotation  space"  for  a  robot  with  a  spherical  wrist.  The 
rotation  space  is  created  by  the  joint  angles  6^,  and  Sg  of  Figure 
3.3.  A  coordinate  frame  defined  at  the  wrist  joint  is  the  reference 
frame  for  the  rotation  space.  Note  that  the  wrist  frame  changes  between 
successive  robot  poses.  But  since  the  wrist  frame  is  not  a  function  of 
the  orientation  angles,  and  can  be  determined  independently  from  the 
tool  pose,  the  rotation  space  angles  9^,  9^  and  9g  are  not  changed. 
We  determine  the  velocity  and  acceleration  constraints  in  rotation  space 
and  separate  rotational  motion  planning  into  tool  rotation  in  rotation 
space  and  the  wrist  rotation  about  the  base  frame.    Since  the  first  3 
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joints  are  basically  the  positioning  joints,  we  can  anticipate  most  of 
the  tool  vector  rotational  motion  planning  in  the  rotation  space. 

In  order  to  demonstrate  the  decoupling  at  a  spherical  wrist,  we 
consider  the  manipulator  of  Figure  3.3  as  an  example.  The  "A"  matrices 
[Denavit  and  Hartenburg  1955,  Paul  1981]  for  this  manipulator  can  be 
derived  as 


A,  = 


Ao  = 


A.  = 


A,  = 


A.  = 


0 

h 

^1  h 

0 

h  ^2 

(3.21) 

0 

1 

0 

h 

0 

0 

0 

1 

-^2 

0 

Cg  I3 

^2 

^2 

0 

Sg  I3 

(3.22) 

0 

0 

1 

0 

J) 

0 

0 

1 

- 

^^3 

0 

C3  1 

4' 

'2 

0 

-^3 

4 

(3.23) 

0 

1 

0 

0 

_p 

0 

0 

1 

0 

-^4 

0  ~ 

^4 

0 

^4 

0 

(3.24) 

0 

-1 

0 

h 

0 

0 

1 

•^5 

-^5 

0 

0 

^5 

0 

0 

(3.25) 

0 

0 

1 

0 

0 

0 

0 

1 
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'^6 

-^6 

0 

0 

H 

^6 

0 

0 

(3.26) 

0 

0 

1 

0 

0 

0 

1 

We  will   show  that  the  velocity  of  the  tool   referred  to  the  base 

coordinate  frame  can  be  decoupled  into  (1)  a  rotational  velocity  of  the 

tool  vector  with  respect  to  the  forearm  (between  elbow  and  wrist)  and 

(2)  a  velocity  (rotational  and  linear)  of  the  wrist  referred  to  the  base 

frame.  [Featherstone  1983,  Hollerbach  1983].     It  is  usual  for  a  robot 

motion  specification  to  contain  the  linear  velocity  of  the  tool  and  the 

rotational    velocity   of  the   tool    frame   expressed   in   the   fixed  base 
B  B 

frame.     Let        ^  and    o)  ^  denote  the  linear  and  rotational  velocities 
of  the  tool  vector. 
From  Fig.  3.3, 


(3.27) 


£w  =   ^t  -  ^ 


Differentiating  Eqn.  (3.27) 

B  •  B  •         B  • 

£-w=     ^t-  ^t 


(3.28) 


where  [T6]  =  ^■^^.^2...I^Q  and    £=[00  l]""". 
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Figure  3.3 


Decoupling  end  effector  velocity 
at  the  wrist  joint 
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Substituting  Eqn.  (3.29)  in  Eqn.  (3.28) 


B  •  B 

Since     2.  I  ^   —  t  ' 


'i„  =  "vj-^^x8[Tj]z  (3.31) 


Eqn.  (3.31)  gives  the  velocity  of  the  wrist.  We  now  determine  the 
velocity  of  the  tool  with  respect  to  the  "wrist  frame".  The  wrist  frame 
B[W]  is  defined  by  the  product  of  the  A  matrices,  A3  and  A^. 

The  rotational  velocity  of  the  tool  frame  is  the  vector  sum  of  the  wrist 

rotational  velocity  and  the  tool  rotational  velocity  with  respect  to  the 

wrist,  all  velocities  referenced  to  the  base  frame. 


B  B       ^  B    W  /o  oo\ 

0)^=     i;)3+w^  (3.32) 


B  W 

where         ^    is  the  rotational  velocity  of  the  tool  from  the  wrist  and 

B  ... 

0)  2  IS    the    wrist    rotational    velocity    (due    to      e^,  9^  and  e^). 

Multiplying  Eqn.  (3.32)  by  ^R^^,  we  transform  all  velocities  in  Eqn. 
(3.32)  to  the  wrist  frame. 


W  W      ^  W    W  (3.33) 

^t  =  ^3^ 


WWW  w 
^t=    ^t  -  ^3 


(3.34) 


90 


(3.35) 


W 


0)  _  is  determined  as  follows: 


Bp     _  B 
-  W 


(3.36) 


-  W 


Substituting  for 


P  ,,  from   (3.31)  and. 


is  knov/n   from  the 


joint  coordinates,  we  can  solve  for  the  3  elements  of  iil  3  Eqn. 
(3.36).    Then    ^lu  _  is  given  as 


Therefore,  we  can  now  determine  the  tool  rotational  velocity  with 

respect  to  the  wrist  frame. 

Thus,  the  tool   frame  rotational  velocity,      o)^,    can  be  divided 

into  2  components.    One  is  the  tool  rotational  velocity  referred  to  the 
B  W 

wrist  frame,     w  ^  ,  also  called  the  velocity  in  rotation  space  and  the 

P 

other  is  the  wrist  rotational  velocity,  _co  ^  ,  referred  to  the  base 
frame.  Since  the  2  rotational  velocity  components  are  independent, 
constraints  on  the  rotational  velocity  of  the  tool  frame  can  be 
transformed  into  constraints  on  the  wrist  rotational  velocity  and  the 
tool  rotational  velocity  from  the  wrist. 

Rotational  Acceleration  of  the  Tool  Vector  in  Rotation  Space 

The  magnitude  of  the  tool  rotational  acceleration  in  rotation  space 
is  limited  by  the  full-load  velocities  and  accelerations  of  the  3 
degrees  of  freedom  in  rotation  space.     In  this  section,  we  derive  the 


W 


(3.37) 
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maximum  "usable"  tool  rotational  acceleration  in  rotation  space.  The 
maximum  usable  magnitude  of  the  tool  rotational  acceleration  is 
independent  of  the  arm  configuration. 

Let  W  denote  a  coordinate   frame  at  the  wrist  joint.      For  the 
manipulator  of  Figure  3.3,  we  define  W  as  follows: 

[W]  =  [Al]  [A2]  [A3]  [T  (z.lg)]  (3.38) 

where  [Al]  ...  [A3]  are  the  A  matrices  given  by  Eqns.  (3.21)  -  (3.23) 

and  T  (z,  Ig)  is  the  translation  from  the  "elbow"  to  the  wrist  joint. 

w  w  * 

The  rotational   velocity      w  and  the  rotational  acceleration       u  are 

given  in  terms  of  the  3  rotation  space  rotations  as 
''^OJ  =  9    z  +  [Rot  (z,  6  )  ^]  e  + 

~  ~  .  (3.39) 

[Rot  (z_,  64)  Rot  (y.  6g)    z]  8g 

i!i  =  64  1+  ^4  ^5  [z.  X  Rot  {z,  9^) 

+  Rot  {z,  9^)  X  Sg  +  64  8g  [z_  X  Rot  (z_,  9^)  Rot  ( 

1'  9^)     z  ]  +  9g  9g  [Rot  {z,  9^)  {y  x  Rot  (x,  ^s))  1  ] 

+  [Rot  {z,  9^)  Rot  (x,  9g)    z  ]  9g  (3.40) 

where  7  =  [0  0  l]"*^  and  ^  =  LO  1  0^^  . 

The   large   number   of   "coupling"   terms    in   the  joint   angles  and 

accelerations,  and  the  dependence  on  the  tool  pose  make  Eqn.  (3.40) 

6 
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difficult  for  a  direct  evaluation  of  the  limiting  magnitude  of  '^uj'  . 
In  order  to  simplify  the  evaluation  of  a  maximum  tool  rotational 
acceleration  that  is  independent  of  the  tool  pose  and  is,  therefore, 
valid  for  all  tool  configurations  in  rotation  space,  we  divide  the 
rotation  in  rotation  space  into  2  components.  One  component  is  a  pure 
rotation  about  the  tool  axis,  the  constraints  on  which  are  known  from 
the  full-load  constraints  on  the  tool  rotation  about  the  tool  axis  (last 
link).  The  second  component  is  due  to  the  remaining  two  rotations,  8^ 
and  85.  Thus,  our  problem  is  reduced  to  the  determination  of 
constraints  on  the  rotation  due  to  8^  and  6^  followed  by  a  combination 
of  these  constraints  with  the  constraints  on  the  tool  axis  rotation,  to 
yield  the  constraints  on  the  overall  rotation.  The  two  accelerations  in 
rotation  space  can  be  thought  of  as  the  tool  approach  acceleration  and 
the  tool  orientation  acceleration,  since  8^,  change  the  tool  approach 
and  8g  varies  the  face  plate  orientation.  Thus,  if  the  rotation  of  the 
tool  in  base  frame  for  a  move  is  divided  into  approach  rotation  and 
rotation  about  the  tool  axis  [Paul  1979,  1981],  transformation  of  these 
2  rotations  into  the  rotation  space  directly  yields  the  divided 
rotations.    Thus  at  any  particular  instant  of  time. 

Rot  (V  \)  =  Rot  (\  ,  3)  Rot  (\  ,  Sg)  (3.41) 

a  b 

where    \  is  the  known  axis  of  tool  rotation  in  the  wrist  frame,    8^  is 

the  known  rotation  angle  about  this  axis,    '^k^   is  the  decoupled  axis  of 

W  ^ 

rotation   due  to   8^  and   85  and      k_   is   the  tool    axis   itself.  The 

solution  for    \    ,    3  from  Eqn.  (3.41)  is  considerably  simplified  if  we 

a  y 
transform  the  equation  to  the  tool  frame  "T"  such  that      k_   becomes  the 

b 
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z-axis  and    \   is  an  axis  obtained  by  rotating  the  tool  frame  y-axis  ^ 
a 

radians  about  the  tool  frame  z-axis  [Paul  1981].  Transforming  Eqn. 
(3.41)  to  the  tool  frame, 


Rot  Ck  ,  e^)  =  Rot  (^k_  ,  e)    Rot  Cz  ,  9^)  (3.42) 


"•"k    =  (-St  c^\>  0)"^   ,  (3.43) 


"^z  =  (  0     0     1)"^  (3.44) 


where  s^,  c<|)  denote  the  sine  and  cosine  of  ^  respectively.  Expanding 
both  sides  of  Eqn.  (3.42)  and  solving  for  ^  and  S  ,  we  get 


\li  =  ATAN2  [(k    k    vers  w  -  k    sin  u) ,    (k    k    vers  oj  +  k    sin  u)] 
r  LVy2  X  "^xz  y 

(3.45) 


2 

sin  g  =  -  S'l'  ci|)  Vers  9g(k^  vers  a)+  cos  u))  + 

(cii^^  vers  0g  +  cGg)  (k^  k^  vers  co  +  k^  sin  w)  (3.45) 

-  sij)  S9-.  (k    k    vers  lo  -  k    sin  u) 
^     6  ^  X    z  y  ' 


cos  3  =  -  St  cip  vers  Sg  (k^  k^  vers  oj  -  k^  sin  u) 

2  2 
+  (ct    vers      +  cSg)  (k^  vers  u  +  cos  oj)  (3.47) 

-  St  se^(k    k    vers  tu  +  k    sin  to) 
6'  y    z  X 


e  =  ATAN2  (sin  3,  cos  g) 


(3.48) 
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Expressed  in  the  wrist  frame,  the  axes  of  rotation  are 


\=  (R4       Ag)"^  [-C^  of  (3.49) 

d 

=  Rot  (\    ,  g)  (3.50) 
b  a 

where 

'^z  =  [0  0  1]"^  (3.51) 


and  R4  is  a  pure  rotation  about  the  wrist  frame  z-axis  by  angle  6^, 
In  the  wrist  frame. 


'^k  (0  =  '^k    e  +  Rot  ("^k  ,  e  )    z    Qr  (3.52) 
a  a  ^ 


Differentiating  Eqn.  (3.52) 


\m    =  \   B"  +  l^k    i  X  Rot  (^'ji  ,  e  )    z    e  ] 
a  a  a 

+  [Rot  (\  ,  B)      z_  ]    Gg  (3.53) 
a 


Let  us  analyze  Eqn.  (3.53)  for  the  magnitude  of  00  .  Notice  the 
fewer  terms  in  Eqn.  (3.53)  as  compared  to  Eqn.  (3.40).  Let 


'''k,   =  [a^  a^  83]^  (3.54) 


and    Rot  (\  ,3)     z_   =  [b^  b^  b^']^  (3.55) 
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The  vectors  in  Eqns.  (3.54)  and  (3.55)  are  unit  vectors. 
Substituting  Eqns.  (3.54)  and  (3.55)  in  (3.53),  taking  magnitudes  and 
simplifying,  we  get 


The  only  negative  term  in  Eqn.  (3.56)  can  be  the  last  term  whose 
coefficient  is  the  dot  product  of  the  unit  vectors  given  by  Eqns.  (3.54) 
and  (3.55).  The  minimum  value  of  the  coefficient  of  this  term  is  -1. 
The  following  observations  can  be  made  from  Eqn.  (3.56):  (1)  the  bound 
on  the  maximum  full-load,  configuration  independent  value  of  |a)|  is 
obtained  when     9,  =  9"  =  0  or  3  =  3"  =  0,  (2)   since  the  maximum  full- 

0  0 

load    bound    on  9"  is    known,    we    now   have   to    determine    the  maximum 
6 

configuration  independent  value  of  3".  The  smaller  of  9g  and 
3"  yields  the  required  magnitude  of  o),  (3)  the  actual  magnitude  of 
0)  as   shown  by  Eqn.    (3.56)   is   always   greater  than  or  equal   to  the 

magnitude  of  the  full-load  configuration  free  values  of  either 
6^  or  3". 

We  now  determine  the  maximum  full-load  configuration  independent 
magnitude  of  3".  Let  tz  denote  the  tool  vector  in  the  wrist 
frame,  tz  is  given  by  the  two  rotations    9  ,  9    in  the  wrist  frame  as 


where  [t^  t^  t^]  denotes  the  initial  (or  a  reference)  tool  orientation 
in  the  wrist  frame.    Without  loss  of  generality,  we  can  assume  [t  ,  t^. 


(3.56) 


W. 


'tz  =  Rot  ("z,  9^)    Rot  (V  9^)    [t^  ty  t^]^ 


(3.57) 
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t,]  =  [0,  0,  1],  or  the  initial  position  of  the  tool  in  the  wrist  frame 


is  along  the  z-axis.  Then, 


tz 


^4  ^5  ^z 
^4  H 
S  ^z 


(3.58) 


where  s^,  c^,  s^,  represent  the  sines  and  cosines  of  9^  and  9^ 
Differentiating  Eqn.  (3.58), 


'tz'  = 


■^4  ^5  ^z 

^4  H  ^ 


C4  C5 1^ 

^4 

^4  H  ^z 

-    Sr  t 

5  z 

_^5^ 

(3.59) 


=  J  [e^  e^]  (3.60) 


where  J  is  the  3x2  Jacobean  matrix.  Now, 


Wtz-  =  B  X  '^tz  .  (^-^1) 


Differentiating  Eqn.  (3.61), 

3"  X  ^^tz  =  ^tz"  -  e  X  e  X  "^tz  (3.62) 


Taking  magnitudes  on  both  sides  of  Eqn.  (3.62), 

I  1"  X  ^tz  I  >  I  ^tz"  I  -  I  i  X  _e  X  V  I  (3.63) 


I 
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e"  I  I  '^tz  1  sin  T  >  1  '^tz"  I  -  I  i  X  i  X  ^tz  I 


(3.64) 


I'tz"  I 


|_e  X  _§  X  tz_| 

l^^tz  I  sin  Y  I'^tz  I  sin  y 


where  y  is  the  angle  between        and  t_z. 
Differentiating  Eqn.  (3.60), 


Y  *  0 


(3.65) 


(3.66) 


Evaluating  Eqn.  (3.66), 


■H  °1  'l^  U  °2  (-^4  «2  '5  -  ^4  "1  '4)  '4 

•   +  (-C4       %  -  S4       e^)  85 


C4  «i  ej  +  S^  +   {C4        65-  s^        O^}  9^ 

•  •  • 

+   {-S^  ct^  65  +  64}  Sj. 


(3.67) 


where 


a,  =  Sc  t 
1       5  z 


(3.68) 


°2  =  S  ^z 


(3.69) 
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Taking  the  magnitude  on  both  sides  of  Eqn.  (3.67), 


*  • 


+  (a^  ej+  2        9^  (ct^'s^^  -        e^^)^  (3.70) 


W  2 

In  order  to  find  the  minimum  |  tz" |  when  the  joint  rotations  6^ 
and  6g  are  driven  at  their  maximum  full  load  acceleration,  we  consider 
the  following  cases. 

(1)     V      =  0 


iVl^  =        (e^"^  +  6"^)  (3.71) 


2         2         2  2 
where        =       +        =  (tool  length) 


(2)  =  0^  =  0 


iVl^  =       sin^  (9  1+  ef  )  (3.72) 


where  y  is  the  angle  between  the  z-axis  of  the  wrist  (also  the  axis  of 
rotation)  and  the  tool  vector.  Note  that  the  y  in  Eqn.  (3.72)  is  the 
same  y  of  Eqn.  (3.65) . 

It     can     be     easily     shown     that     any     other     combination  of 
9^,  6^,  9g,  9^  is  either  greater,  or  can  be  derived  from  Eqns.  (3.71) 
and  (3.72).     Thus,  the  bound  on  the  maximum  configuration  independent 
value  of    I  ^tz"  |  occurs  when  either    9^  does  not  move  or  when    9^  is 
fixed. 
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*  *  \  I 

The  second  term,  |  1  x  _3  x  tz_  |  ,  in  Eqn.  (3.65)  is  also  bounded 
on  its  maximum  tool  configuration  independent  value  by  either        =  0  or 


=  0  as  shown  below. 

0 


_e  X  i  X  *^tz  I  =  |_3|^  |tz|    sin  k  (3.73) 


where    <  is  the  angle  between    _B  and  tz_.      We  determine  by  taking 

magnitudes  on  both  sides  of  Eqn.  (3.61), 

Ttz'  I  =  ill  \tz  I  sin  <  (3.74) 


|3|=-— -  (3.75) 


we  derive    I'^tz^' |  from  Eqn.  (3.59)  as 


I  \z'   1^  =        e/+   (a2+  al  )  (3.76) 


where  a^,  are  given  by  Eqns.  (3.68)  and  (3.69).  Eqn.  (3.76)  shows 
that  |^tz_'|^  is  a  sum  of  two  squares.  If  both  terms  on  the  right  hand 
side  of  Eqn.  (3.76)  are  zero,  then  we  get  the  trivial  solution, 
I'^tz '  I  =  0.  The  lower  bound  on  the  maximum  value  of  |^tz_' |  is  given 
by  making  one  of  the  two  terms  zero  at  a  time.  Thus, 


(3.77) 
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l^tz'  1^  =  T,_^  sin^  K  6^^  0g=  0  (3.78) 

We  conclude  from  Eqns.  (3.73),  (3.75),  (3.77)  and  (3.78)  that  the 

*      *  w 

maximum  configuration   free   (lower  bound)    value  of     ll  x  _3  x    tzj  is 

•  • 

given  when  either    e^=  0  or  9^=  0. 

lixix^tzl  =  (T^  e^^)  /(sin  k)  e^=  0  (3.79) 

or  li  x  _3  x  '^tzl  =       9^  sin^<  9^=  0  (3.80) 

Substituting  Eqns.  (3.71),  (3.72),  (3.79),  (3.80)  in  Eqn.  (3.65), 

|e"|  >    [  (9g^  +  6'^^)  /  sin  Y  -  e^^/  sin^  ]  =  ej  =  0  (3.81) 

>     [  (64^  +  Qf)  -  e^^]  %  =  ^5=0  (3.82) 

Note  that  sin  <    =  sin  y  due  to  the  conditions  imposed  on  the  axis 
of  rotation.     In  Eqn.  (3.81),    sin  y  =  1  because  the  angle  between  the 
axis  of  rotation  and  the  tool  vector  is  always    |ti/2|  radians  due  to  the 
9^  =  0  condition  on  rotation.    Therefore,  we  can  rewrite  Eqn.  (3.81)  as 

1 3"  I  >  1{Q^^  +  ef  )  -  Q^:\       0^  =  ej  =  0  (3.83) 

The  smaller  of  the  two  terms  on  the  right  hand  sides  of  Eqns. 
(3.82)  and  (3.83)  gives  the  limiting  magnitude  of  the  maximum  _3". 

Thus,  returning  to  Eqn.  (3.56),  the  configuration  independent 
magnitude  of  the  angular  acceleration  of  the  tool,     | a)| ,  is  limited  by 
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the  smaller  of    9"  or  g".      Since    g"  is  again  limited  by  the  slower  of 

0 

6"  and    e"    we  conclude  that  the  tool  vector  rotational  acceleration  in 
4  b 

rotation  space  is  limited  by  the  slowest  rotational  joint. 

Thus,  summarizing  the   results  of  this  section,  we  decoupled  the 

rotation  in  the  rotation  space  into  2  independent  rotations  in  order  to 

simplify  the  estimation  of  the  rotational  tool  acceleration.    We  derived 

an    expression    for    the    rotational    acceleration     o)     in    terms  of 

e",  e,  and  6"  where    3"  is  a  function  of  the  first  two  Euler  rotations, 
0  b 

(j)  and  e,  and  8g  is  the  acceleration  of  the  rotation  about  the  tool 
axis  itself.  The  equations  for  u,  3"  are  simple  enough  to  be  computed 
in  real-time  for  an  instantaneous  acceleration.  In  other  words,  given  a 
tool  pose,  rotational  velocity  oj,  and  joint  velocities  and 
accelerations,  we  can  determine  the  instantaneous  value  of  the  tool 
rotational  acceleration  u.  Since  the  value  of  to  continually  changes 
with  the  tool  pose,  we  determined  a  magnitude  of  u  that  is  less  than  or 
equal  to  the  maximum  value  of  |u)|  for  any  tool  pose  in  the  wrist 
frame.  We  proved  analytically  that  |a)|  is  given  by  the  rotational 
acceleration  of  the  slowest  joint  in  the  rotation  space. 

Point  to  Point  Unconstrained  Transitions 

For  a  point  to  point  unconstrained  transition,  the  position  and 
velocity  at  the  two  ends  of  the  transition  interval  are  known.  We  have 
to  determine  an  independent  move  time  for  the  tool  orientation  to  change 
from  its  initial  frame  and  velocity  to  the  destination  frame  and 
velocity.  Let  us  assume  that  the  tool  is  rotating  about  an  axis 
k_  ^  with  an  "angular  velocity  _k  ^  .  Let  it  be  required  that  the 
tool    change    its    axis    of    rotation   to     k_  ^  and    angular   velocity  to 
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"2  —  2*      '■^^  '^l  start  point  of  the  transition  and  P2  the 

destination  point.  At  P-^  and  the  linear  and  rotational  velocity 
constraints,  ±  -^t  ^  2  "^1  —  1'  '^2  —  2  ''^^  easily  transformed  to 
the  velocities  of  the  orientation  angles  of  the  spherical  wrist 
manipulator  by  initially  decoupling  the  velocities  at  the  wrist,  as  in 
the  preceding  section,  and  then  solving  for  the  joint  velocities  in 
rotation   space   [Featherstone   1983,   Hollerbach   and  Sahar  1983],  Let 

•  •  •  •  •  • 

(<t>^,  e^,  4)^,        e^,  ^^)^)        and  {^^,  e^,        *2'  denote  the 

boundary  conditions  at  P-^  and  P2  respectively  for  the  rotational 
transition  in  rotation  space.     From  the  specifications  of  the  joint 

motors  of  the  orientation  angles,  we  know    (9^,„  ,  6"  ,  ,  ^"^  ),  the 

max       max  max 

servo  accelerations  at  full  load.  If  a  cubic  polynomial  is  used  for 
each  of  the  orientation  angles  (in  order  to  keep  the  position  and 
velocity  continuous  at  P^  and  P2),  we  can  determine  a  move  time  "Tp"  for 
each  orientation  angle  such  that  the  constraint  on  maximum  acceleration 
is  not  violated. 
Let 

3  2 

f(u)  =  c^  u   +       u   +  c^  u  +  Cj  (3.84) 

denote  a  cubic  polynomial  for  one  of  the  orientation  angles,  "u"  is  the 
normalized  time,  given  as 

u  =  (t  -  t._i)/T._^  (3.85) 


Ti.l=^i  -^i-l 


(3.86) 


103 

where  T^_-^  is  the  time  for  transition  between  P|-P2  i^i'^z  denoted  as 
the  ith  time  interval  in  the  series  of  time  intervals  for  the  robot 
move).     Let    (y^  ^,  Y^-  ,  Y^- )  denote  the  boundary  conditions  at  P-^ 

and  P2.  The  coefficients  of  f(u)  can  be  derived  in  terms  of  the 
boundary  conditions  as 


C3  =  3  (Ay.)  -  (y^  +  2  y._^)  T._j  (3.88) 


C4  =  -2  (Ay.)  +  (y^.  +  T._^  (3.89) 


where    Ay.  =  y.  -  Y^_j. 

Since  the  maximum  acceleration  for  f(u)  can  occur  at  either  u=0  or 
u=l,  we  evaluate  T^,^  at  both  ends  of  the  ith  time  interval  (P^  -  P2). 
The  equation  for  the  acceleration  y"  is  obtained  by  differentiating 
f(u)  twice. 


y"  T?_^=  6  c^  u  +  2c2  (3.90) 


Substituting  for  c^  and  C2  and  solving  for  T^-.^  at  u=0  and  u=l,  we 

get 


-  (4y.  1  +  2y. )    t  /  4t^.  ^  +  2y.)^  +  24  y"  Ay. 


(3.91) 


104 


u=l 


(2  Y^_^  +  4  Y.)    t  /  (2  +  4  Y^)^  +  24  y"  Ay- 

-2y" 


(3.92) 


Only  one  of  the  above  values  for  T^_2  gives  a  positive  real  root. 
Thus,  the  move  time  for  a  point  to  point  rotational  transition  can  be 
determined  independently  by  computing  T^_-|^  for  each  of  the  3  orientation 
angles,  and  using  the  minimum  value. 

Independent  Rotations  -  An  Illustrative  Example 

In  order  to  demonstrate  the  application  of  the  results  of  the 
previous  sections,  we  consider  a  robot  move  where  the  conventional 
translation  dominant  trajectory  planning  techniques  fail  to  produce  a 
smooth  rotation  of  the  robot  tool.  The  robot  used  for  simulation  is 
shown  in  Figure  3.3  The  move  for  simulation,  shown  in  Fig.  3.4,  is 
specified  as  follows.  The  robot  end  effector  should  move  continuously 
along  a  linear  path  from  a  point  P-^  and  initial  orientation  R-^  to  an 
intermediate  point  P2  with  rotation  R2  and,  finally,  to  a  point  P3  with 
orientation  R3.  The  linear  velocity  of  the  tool  tip  (assumed  to  be  the 
tip  of  the  last  link)  along  P1-P2  and  P2-P3.  is  a  constant  0.15  m/sec. 
The  points  P^,  P2  and  P3  are  chosen  such  that  they  are  colinear.  Since 
the  linear  velocity  along  P1-P2  and  P2-P3  is  the  same  in  magnitude  and 
direction,  no  smoothing  of  the  translational  trajectory  is  required  at 
P2.  The  rotations  at  P-^,  P2  and  P3  are  chosen  such  that  the  rotational 
velocity  of  the  tool  is  quadrupled  along  P2-P3  (over  the  one  along  P^- 
P3).    Figure  3.4  shows  the  move  and  move  parameters. 
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Figure  3.4        A  simulation  move 
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A  well-known  technique  for  position  and  rotation  interpolation 
along  a  linear  end  effector  trajectory  [Paul  1979,  1981]  is  employed  for 
generating  joint  coordinates  at  a  fixed  sample  rate  on  the  linear  tool 
tip  trajectory.  A  sample  rate  of  50  ms  is  chosen,  since  this  value  is 
representative  of  the  typical  sampling  rate  on  industrial  robot 
controllers.  The  rotational  motion  is  divided  into  approach  vector 
rotation  and  approach  vector  orientation.  The  first  rotation  serves  to 
rotate  the  tool  vector  from  its  initial  orientation  to  the  final 
orientation.  The  second  rotation,  which  is  about  the  tool  axis,  aligns 
the  "face-plate"  axes  (the  2  axes  of  the  tool  frame  perpendicular  to  the 
tool  vector)  from  their  initial  orientation  to  the  final  orientation 
[Paul  1979,  Paul  1981].  The  continuous  linear  motion  from  P^  to  P3  is 
computationally  divided  into  two  linear  moves,  ^1-^2  ^2'^3'  '^^^ 
"flyby"  time  at  P2  for  the  continuous  move  using  a  translation  dominant 
approach  is  given  as: 

ll  1  "111 

T  =  i  ^  =  0  (3.93) 

max 

where  _v_  ^  is  the  velocity  along  P1-P2  and  P2-P3  and  a^^g^  is  the  maximum 
linear  acceleration.  Thus,  at  P2  no  smoothing  is  done.  Since  the 
rotational  velocity  is  quadrupled  on  the  segment  ^2~^3'  ^  J'^'^P  ^'"^ 
orientation  angles  will  be  observed  at  P2.  The  translation  dominant 
approach  cannot  smooth  this  jump  in  the  orientation  angles.  The  plots 
of  the  joint  trajectories  in  Figures  3.5  to  3.10,  where  the  solid  line 
trajectory  indicates  a  translation  dominant  move  planning,  shows  a  jump 
in  the  angles  6^,  and  8g.  The  plots  for  9^,  and  6^  show  that 
the  jump  due  to  an  instantaneous  change  in  the  rotational  velocity  does 
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not  produce  significant  jumps   in  the  first  3  angles,  since  the  end 

effector  position  and  linear  velocity  are  continuous. 

II  II 

The  rotational  accelerations,  B      (Eqns.  (3.82),  (3.83))  and  Og  , 

are  employed  to  smooth  the  discontinuity  in  the  rotational  velocity  at 

At  ?2,  let    Aiiiapj    ^— or  differences  in  the  tool  approach 

and   orientation    velocities    along    P1-P2   ^nd    P2"^3'  rotational 

velocity  jumps,    Au      and  Au      are  transformed  in  to  velocity  jumps  in 
"~  a p         —  or 

the  wrist  frame,    ^Au)      and  ^Aui       .    Two  transition  times  t,,,  and  t-^, 

—  ap  —  or  ap  or' 

for  smoothing  the  velocity  jumps  at  the  maximum  permissible  acceleration 
(independent  of  the  tool  pose)  are  determined  as  follows: 


The  greater  of  the  two  transition  times,    x     and    x    ,  is  the  time 

ap  or' 

for  smoothing  the  jump  in  the  rotational  velocity  at  P2.  Let  x^  denote 
the  greater  of  x^^  and  x^^.  Then,  the  rotational  velocity  is  changed 
smoothly  from  x^/2  seconds  before  P2,  to  x^/2  seconds  after  P2.  The 
intermediate  rotations  are  given  by  the  following  equation. 


R(t)  =  R2  [Rot  [k  ^p^,  -  {(x^  -  t)2  co^p^}  /  2x^]  Rot  [k 

-  -  ^^''-orl^  /  ^^'^  (iap2'  ^Sp2/ 

Rot  (k  ^^2'       V2/  2^)1 

0  <  t  <  x  (3.96) 
r 
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The  dashed  lines  in  Figures  3.5  to  3.10  show  the  joint  trajectories 
due  to  smoothing  in  rotational  space  by  Eqn.  (3.96). 

A  Note  on  the  Wrist  Rotational  Velocity  in  Base  Frame 

In  the  section  on  planning  constrained  rotation  transitions,  we 
showed  that  the  tool  rotational  velocity  can  be  decoupled  into 
rotational  velocity  of  the  tool  about  the  wrist  frame  and  the  rotational 
velocity  of  the  wrist  about  the  base  frame.  We  also  described  the 
determination  of  constraints  on  the  rotational  velocity  of  the  tool  in 
the  wrist  frame.  In  this  section,  we  briefly  address  the  determination 
of  the  constraints  on  wrist  rotation  about  the  base  frame.  Since  the 
first  3  joints  are  primarily  the  "positioning"  joints,  and  the 
limitations  on  the  tool  linear  velocity  and  acceleration  are  known,  we 
know  that  the  linear  velocity  and  acceleration  limits  of  the  tool  tip 
apply  to  the  wrist  also.  The  linear  velocity  and  acceleration 
constraints,  that  are  valid  for  all  moves  of  the  tool,  become  those  of 
the  wrist  when  the  orientation  angles  do  not  change,  and  the  wrist 
trajectory  is  parallel  to  the  tool  tip  trajectory.  The  wrist  rotational 
velocity  can,  therefore,  be  estimated  from  the  equation: 


'iw    =  (3.97) 

e 

where      r     is  the  wrist  position  vector  in  the  base  frame.    The  worst 

w 

case  value  of  I  i!i  ,^1  can  be  determined  by  finding  the  closest  position 
of  the  wrist  to  the  robot  base  frame.  The  rotational  acceleration  can 
be  estimated  by  differentiating  Eqn.  (3.97). 
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For  manipulators  with  the  kinematic  structure  of  Figure  3.3,  the 

wrist  rotational  velocity  is  primarily  limited  by  the  "waist"  rotation 

6^.    This  joint  is  generally  the  slowest  of  the  6  joints  in  Figure  3.3, 

because  for  most  moves,  the  loading  on  joint  1  is  the  maximum.  Thus, 

another  estimate  of  the  limit  on  the  wrist  rotational  velocity  is  the 

maximum   full -load   speed   of   the  waist   joint.      The  wrist  rotational 

acceleration  is  the  rotational  acceleration  of  the  waist  joint.  The 

wrist    rotational    velocity    and    acceleration    thus    estimated   must  be 

greater  than,   or  equal    to  the   rotational    velocity  and  acceleration 

R  R 

determined  from  Eqn.  (3.97),  since  |  _v  ^|  and  |  a_  ^|  are  the  estimates 
of  the  linear  velocity  and  acceleration  that  are  independent  of  the  arm 
configuration  and  the  move. 

For  a  robot  move  that  has  a  translation  and  rotation,  if  the 
translation  trajectory  is  continuous  in  position  and  velocity,  a 
discontinuity  in  the  rotational  velocity  will  mostly  appear  in  the 
orientation  joints  (9^,  9^  and  9g  in  Figure  3.3).  This  is  illustrated 
in  the  simulation  example  of  the  preceding  section.  However,  if  a  move 
is  purely  rotational,  as  in  rotation  of  the  tool  about  one  of  the  tool 
frame  axes  keeping  the  tool  point  fixed  (a  common  feature  on  most 
industrial  robot  "teach  pendants"),  the  wrist  rotational  acceleration 
may  be  the  determining  factor  for  the  move  time.  In  general,  for  any 
robot  move,  the  limitations  on  the  rotational  acceleration  must  be 
checked  in  the  base  frame  for  the  wrist,  and  in  the  wrist  frame  for  the 
t joI  .  The  greater  of  the  two  move  times  limit  the  rate  of  change  of  the 
tool  rotation. 


CHAPTER  IV 

TRAJECTORY  SPECIFICATION  AND  LOAD  DISTRIBUTION 
FOR  CLOSED-LOOP  MULTI-MANIPULATOR  SYSTEMS 

In  Chapters  II  and  III,  we  described  a  new  technique  for  on-line 
polynomial  interpolation  in  joint  space  and  developed  the  concept  of 
independent  robot  motion  planning  in  rotation  space.  Thus,  given  a 
multi-manipulator  task,  we  can  plan  constrained  motions  (translations 
and  rotations)  of  the  end  effector  such  that  the  motion  does  not  demand 
excessive  joint  rates  or  joint  accelerations  for  either  translation  or 
rotation  dominant  moves.  With  the  "feasible"  motion  planned  by 
independent  translation  and  rotation  trajectories,  we  can  generate  joint 
trajectories  between  successive  sample  periods  on  the  end  effector 
trajectory  employing  the  on-line  cubic  splines  technique  of  Chapter 
II.  If  the  robots  are  working  on  independent  sub-tasks,  a  fast  and 
unconstrained  tool  transition  between  work  points  can  be  executed 
efficiently  with  a  controlled  start-up  and  set-down  jerk  by  the  joint 
model  of  Chapter  II,  which  clips  both  velocities  and  accelerations  of 
the  non-pacing  joints.  In  this  Chapter,  we  address  one  of  the  problems 
in  the  coordination  of  a  "closed-loop"  multi-robot  task,  namely,  the 
load  distribution  at  the  end  effectors  in  the  closed-loop. 

A  large  number  of  tasks,  from  small  item  assembly  to  displacement 
of  large  objects,  can  be  made  faster  and  more  efficient  with  coordinated 
movements  of  multiple  robots  working  in  a  closed-loop.  In  a  closed-loop 
task,  as  in  the  displacement  of  a  long  rod  by  two  manipulators,  an 
active  distribution  of  the  forces  and  moments  at  the  end  effectors  is 
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required  by  most  control  techniques  for  the  determination  of  the  motor 
torques  of  each  manipulator  in  the  closed  loop.  Several  control  schemes 
have  appeared  in  the  literature  for  closed-loop  control  of  multiple 
manipulators  in  a  closed  chain,  or  mechanisms  similar  in  configuration 
to  the  closed-loop  multi-manipulator  system  [Hemami  and  Wyman  1979, 
Ishida  1977,  McGhee  and  Orin  1976,  Orin  and  Oh  1981].  For  discussion, 
we  broadly  classify  the  control  techniques  into  3  categories:  (1)  force 
control,  (2)  hybrid  control  and  (3)  computed  torque-position  control. 
In  a  force  control  scheme,  the  joints  are  servoed  on  force  feedback 
instead  of  the  usual  position  and  velocity  in  a  traditional  P-I-D 
loop.  The  system  predicts  the  joint  torques  for  a  given  trajectory  from 
force  sensors  located  on  the  joints  or  at  certain  parts  of  the 
manipulator.  The  joint  motors  are  servoed  on  force  feedback  and  the 
system  attempts  to  build  up  the  torques  on  the  motors  to  counter  the 
sensed  forces. 

Hybrid  control  of  a  manipulator  is  defined  as  a  control  scheme  that 
employs  position  servoing  on  some  joints  and  force  servoing  on  others 
[Paul  and  Shimano  1976,  Raibert  and  Craig  1981].  The  primary 
application  of  hybrid  control  as  in  [Paul  and  Shimano  1976,  Raibert  and 
Craig  1981]  is  for  controlling  compliant  motions  of  a  robot  end 
effector.  The  combined  force-position  servoing  technique  of  hybrid 
control  has  been  applied  to  multi-manipulator  control  also.  The  idea  is 
to  create  a  master/slave  control  [Ishida  1977]  where  the  master  is 
position  servoed,  and  the  slave  .s  force  servoed.  The  master  slave  idea 
works  well  as  long  as  the  attachment  between  the  object  and  manipu- 
lators is  absolutely  rigid,  and  no  compliance  is  possible.  If  there  are 
degrees  of  freedom  between  the  object  and  an  end  effector,  then  the 


118 

sensed  forces  on  the  effector  will  not  accurately  predict  the  notion  of 
the  system.  In  this  case,  the  slave  is  bound  to  loose  position  accuracy. 

The  computed  torque  control  method  determines  the  torque 
requirement  of  each  motor  by  a  dynamic  model  [Luh  1980].  The  torques  on 
each  joint  are  constantly  updated  from  the  position  and  velocity 
information  from  the  trajectory,  and  feedback  from  the  servo.  Since  the 
joint  torques  are  a  function  of  the  arm  pose,  the  computed  torque 
control  technique  yields  good  position  control.  Thus,  in  case  of 
slippage  at  the  end  effectors,  the  current  robot  position  is  updated 
from  position  feedback  and  incorporated  into  the  new  computation  of  the 
joint  torques. 

Since  the  joint  torques  are  a  function  of  the  external  forces  and 
moments  on  the  effectors,  we  require  a  model  to  predict  the  forces  and 
moments  on  each  end  effector  in  the  closed  chain.  At  each  sample  point 
on  the  object  trajectory,  the  object  load  must  be  distributed  among  the 
effectors  holding  the  object.  Research  on  closed  kinematic  chains  in 
locomotion  systems  like  bipeds  and  hexapods  has  shown  that  the  problem 
of  load  distribution  for  a  given  trajectory  of  the  closed  chain  is  under 
specified  [Orin  and  Oh  1981,  Seireg  and  Arvikar  1975,  Williams  and 
Seireg  1979].  Linear  programming  using  the  Simplex  method,  with 
minimization  of  power  was  implemented  for  solving  the  load  distribution 
problem  for  a  hexapod  locomotion  vehicle  [Orin  and  Oh  1981].  The 
computation  of  torques  for  driving  the  hexapod  on  a  given  trajectory  was 
1.37  seconds  per  sample  point  on  a  PDP-11/45. 

In  this  Chapter,  we  describe  the  task  and  constraint  specification 
for  a  closed-loop  multi-manipulator  task,  and  develop  two  linear  models 
for  load  distribution  in  the  closed-loop.    Both  models  employ  the  space 
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curve  trajectory  of  the  object  to  create  coordinate  frames  with  axes 
along  the  space  curve  tangent,  normal  and  bi-normal  directions.  It  is 
shown  that  the  use  of  the  space  curve  coordinate  frames  simplifies  the 
constraints  on  the  load  distribution  problem.  The  first  of  the  2  linear 
models  simplifies  load  distribution  by  separating  the  forces  fo-r 
translation  and  moments  for  rotation  of  the  object.  This  model  is 
applicable  to  tasks  performed  at  a  speed  low  enough  for  the  angular 
velocity  effects  on  the  transl ational  force  to  be  ignored.  The 
simplicity  of  the  algorithm  allows  forces  and  moments  to  be  balanced  at 
a  fast  rate  during  the  actual  motion  of  the  system.  The  second  model 
takes  into  account  the  effects  of  all  determinate  forces  on  the  object 
and  generally  would  require  a  large  table  when  linear  programming  is 
employed  for  solution. 

Both  methods  do  not  directly  account  for  the  interactive  forces  due 
to  the  simultaneous  motion  of  all  the  manipulators.  Since  forces  will 
be  re-distributed  as  the  system  moves,  it  is  assumed  that  the  error  in 
force  balance  at  a  certain  time  instant  can  be  sensed,  and  incorporated 
into  the  next  force  balance,  thereby  avoiding  the  possibility  of  a 
steady  error  build  up. 

Planning  a  Multi-Manipulator  Task 

A  multi-manipulator  task,  like  any  other  robot  task  can  either  be 
pre-planned  or  executed  on-line.  If  the  task  is  preplanned,  then  the 
object  and  end  effectors  trajectories  are  computed  and  stored.  The  task 
planning  procedure,  in  addition  to  planning  an  "optimal"  trajectory  [Luh 
1981],  must  ensure  that  (1)  the  trajectories  of  the  manipulators  do  not 
cause  a  collision  among  the  links  of  the  manipulators  or  other  static 


120 

obstacles,  (2)  each  end  effector  trajectory  is  within  the  usable 
workspace  of  the  respective  manipulator,  and  (3)  the  trajectory 
precludes  singularity  points  and  limit  points  in  the  manipulator 
workspace.  Planning  collision  free  manipulator  trajectories  is  a  well 
researched  subject  and  several  techniques  for  collision  avoidance  have 
appeared  in  the  literature.  One  of  the  first  published  works  on 
obstacle  avoidance  is  by  Udupa  [1977].  Udupa  first  formulated  the 
obstacle  avoidance  problem  in  terms  of  an  obstacle  transformation  that 
allows  treating  the  moving  object  as  a  point.  Research  on  obstacle 
avoidance  at  the  MIT  Artificial  Intelligence  Laboratory  has  provided 
greater  insights  into  planning  obstacle  free  trajectories  [Lozano-Perez 
and  Wesley  1979,  Lozano-Perez  1981].  Techniques  for  planning  optimal 
trajectories  by  workspace  constraints  of  a  manipulator  are  also  known 
[Luh  and  Lin  1981].  Planning  a  trajectory  that  precludes  singularity 
points  [Taylor  1979]  in  an  articulated  manipulator's  workspace  is  not  an 
easy  task.  A  singularity  point  causes  excessive  joint  motions  in  some 
joints  of  the  manipulator  for  relatively  small  displacements  of  the  end 
effector  in  Cartesian  space.  Thus,  when  an  end  effector  passes  through 
a  singularity  point,  the  sudden  increase  in  joint  rates  may  result  in  an 
unpredictable  and  jerky  motion  of  the  end  effector.  In  a  closed  loop 
multi-manipulator  task,  this  could  mean  excessive  forces  on  the  object 
and  might  even  cause  the  object  to  slip  and  drop.  There  is  no  known 
straight-forward  method  for  checking  and  handling  singularity  points  in 
the  manipulator  workspace. 

If  the  planning  and  execution  of  a  multi-manipulator  task  are 
simultaneous,  then  considerations  such  as  constructing  minimal-work 
trajectories  between  2  points  are  important.     For  example,  there  are 
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usually  several  ways  of  executing  a  motion  involving  a  translation  and 
rotation.  The  rotation  can  be  uniform  or  non-uniform.  The  deter- 
mination of  an  optimum  motion  strategy  should  take  into  consideration 
the  limitations  on  individual  manipulator  work-spaces  and  physical 
limits  on  the  manipulator  joints  such  as  the  maximum  motor  torques. 

The  next  two  sections  list  the  task  and  constraint  description 
parameters  for  planning  a  multi-manipulator  task  and  describe  the 
trajectory  specification  for  a  closed-loop  task. 

Task  and  Constraint  Description 

The  following  is  a  list  of  parameters  that  must  be  specified  as 
task  description  for  object  manipulation  by  multiple  manipulators.  The 
task  and  constraint  description  parameters  are  used  both  for  trajectory 
planning  and  force-torque  balance. 

(1)  Object  Parameters: 

Object  parameters  such  as  mass,  location  of  center  of  mass  with 
respect  to  a  Universal  or  Base  frame,  and  orientation  of  principal  axes 
with  respect  to  the  Universal  frame  must  be  known. 

(2)  Manipulator  Parameters: 

Manipulator  parameters  include  physical  and  geometric  constraints 
such  as  mass  and  structure  of  the  links,  maximum  joint  rates, 
accelerations  and  torques,  and  the  limits  on  the  usable  workspace. 

(3)  Effectors  grasp  positions  and  orientations: 

Initial  start-up  configuration  of  the  effectors  is  made  a  task 
specification  in  this  formulation.  Various  factors  based  on  object 
parameters  can  be  used  to  determine  an  optimal  start-up  configuration  of 
the  effectors  for  the  given  task.  The  problem  of  determining  an  optimal 
start-up  configuration  is  not  addressed. 
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If  the  contact  between  the  object  and  the  effectors  is  not  rigid, 
then  the  number  of  degrees  of  freedom  between  the  object  and  an  effector 
should  be  specified.  In  this  chapter,  we  address  only  rigid  contact. 

(4)  Normal  vectors  at  the  point  (or  surface,  depending  on  the  type  of 
gripper)  of  contact  between  an  effector  and  the  object: 

The  normal  vectors,  expressed  in  the  frame  at  center  of  mass 
(principal  axes),  define  the  surfaces  of  contact  between  the  effectors 
and  the  object.  A  limit  on  the  maximum  value  of  the  normal  forces  must 
be  specified  in  order  to  prevent  deformation  of  the  object. 

(5)  Start  and  destination  position  and  orientation  of  the  object: 

The  start  and  destination  object  pose  is  given  in  terms  of  the 
object  center  of  mass  and  principal  axes  with  respect  to  the  Universal 
frame. 

(6)  Move  parameters: 

Move  parameters  include  the  linear  and  rotational  velocity  of 
transfer,  and  initial  and  set-down  acceleration  of  the  center  of  mass. 

(7)  Load  sharing  constraints: 

In  load  sharing  constraints,  one  may  specify  the  pay-load 
capabilities  of  the  individual  manipulators.  This  information  is  used 
in  the  force  distribution  algorithm  to  distribute  forces  such  that  the 
effectors  with  the  heavier  pay-load  capability  handle  greater  forces. 

Trajectory  Specification  for  a  Multi-Mani pulator  Task 

A  trajectory  of  the  object  is  conveniently  specified  by  describing 
the  translational  motion  of  the  object  center  of  mass  and  the  rotation 
of  the  principal  axes  with  respect  to  a  universal  frame.  Since  the 
positions  of  the  end  effectors  holding  the  object  are  fixed  with  respect 
to  the  principal  axes  at  any  instant  of  time,  i.e.  rigid  contact,  the 
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motion  of  each  effector  can  be  determined  from  the  motion  of  the  object 
center  of  mass  and  the  principal  axes. 

The  translational  and  rotational  motion  of  the  object  is  specified 
by  six  polynomials,  each  a  function  of  time.  Three  polynomials  describe 
the  translational  motion  of  the  object  center  of  mass.  The  polynomials 
for  the  rotation  of  the  principal  axes  describe  the  rotations  about  each 
one  of  the  3  axes  of  the  universal  frame.  In  order  to  reduce  the  degree 
of  the  polynomial  for  a  large  number  of  via  points  of  the  object, 
piecewise  splined  polynomials,  continuous  in  position,  velocity  and 
acceleration,  can  be  employed  for  storing  the  coefficients.  Note  that 
the  polynomials  can  be  discontinuous  at  any  point  in  time. 

Forces  And  Torques  on  an  Effector  in  a  Closed  Multi-Manipulator  Chain 

In  this  section,  we  identify  the  forces  acting  at  each  effector 
during  the  motion  of  the  object.  The  force  and  moment  at  the  object 
center  of  mass  is  a  composition  of  the  forces  and  moments  on  the 
effectors  holding  the  object.  Since  the  objective  of  the  load 
distribution  is  to  distribute  the  force  and  moment  at  the  center  of  mass 
into  forces  and  moments  on  the  effectors,  we  derive  in  this  section  the 
equations  for  the  center  of  mass  force  and  moment  in  terms  of  the  object 
linear  and  rotational  velocities  and  accelerations.  Since  the 
resolution  of  the  force  and  moment  at  the  center  of  mass  is  not  unique, 
various  criteria  such  as  minimization  of  the  input  power  must  be 
employed  for  load  distribution.  The  last  sub-section  describes  the 
criteria  for  dynamic  load  distribution  in  a  multi -manipulator  task. 
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Classification  of  Forces  on  an  Effector  in  a  Closed-Loop 

Forces  on  the  tips  of  effectors  (points  of  contact  between  object 
and  effectors)  in  a  closed  loop  multi -manipulator  system  can  be 
classified  into  the  following  types: 

(1)  Normal  tip  reaction  forces.  The  tip  reaction  forces  keep  the 
object  from  slipping  and  falling.  Excessive  tip  reaction  forces  can 
deform  the  object. 

(2)  Linear  translational  forces.  These  forces  are  responsible  for 
the  translational  acceleration  of  the  object. 

(3)  Forces  to  counter  gravitational  force  and  forces  due  to 
friction  at  contact  points.  Frictional  forces  are  ignored  in  this 
formul ation . 

(4)  Forces  arising  due  to  the  interactive  coupling  of  the 
manipulator  chains  when  the  object  is  in  motion. 

The  interactive  forces  between  sample  points  on  the  object 
trajectory  are  difficult  to  predict.  The  interactive  forces  are  due  to 
a  lead  or  lag  in  each  manipulator's  motion  on  the  trajectory  between 
points.  This  lead  or  lag  can  be  due  to  backlash  or  gear  friction  or 
slower  response  time  of  certain  joints  of  a  manipulator.  A  simple  way 
to  account  for  the  unpredictable  forces  of  interaction  is  to  sense  each 
end  effector  force  by  means  of  a  wrist  force  sensor  or  any  other  force 
sensor,  and  add  the  interactive  force  to  the  force  due  to  the  load 
distribution  at  each  sample  point. 

Derivation  of  the  Equations  for  the  Force  and  Moment  at  the  Object 
Center  of  Mass 

In  this  section,  we  derive  the  equations  for  the  force  and  moment 
on  the  object  at  any  instant  of  time.     Let  B  denote  a  fixed  frame  of 
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reference  for  the  multi -manipulator  task.     Let  F_  be  the  translational 

force    (independent    of    gravity,    gravity    effect    can    be  compensated 

separately)  at  the  center  of  mass  of  the  object.    The  frame  of  reference 

for    F  is  the  base  frame  B.    If    v       denotes  the  translational  velocity 
—  —  cm 

vector  of  the  center  of  mass,  v_  the  absolute  acceleration  of  the  center 
of  mass  in  the  base  frame,  and  m  the  mass  of  the  object,  then  the 
equation  for  the  linear  force  F_ is 

F  =  mv  (4.1) 


Let 


1=  Fxi+  \  ^  (4.2) 


and 


V  L  =      i+  Vy  j  +  v^  l<_  (4.3) 


where  j_,  j_,  k_  are  unit  vectors  in  the  directions  of  the  three  axes  of 
the  base  frame. 

The   absolute   acceleration     (v)  of   the   center   of   mass    of  the 

object    referred  to  the  base  frame  is  obtained  from  the  linear  velocity 

1.  rm  »   linear   acceleration     a       ,  angular   velocity     to       ,  angular 

cm  —  cm 

acceleration  u  and  unit  vector  r_  (from  the  origin  of  the  base  frame 
to  the  center  of  mass  as  shown  in  Figure  4.1)  as 

I  =  lcm^  iiicm'^  (^cm^il)  ^'^^L  (4.4) 
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Figure  4.1        Translation  and  rotation  of 
object  about  the  base  frame 


where 
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=v    i+v    j+v    k  (4.5) 
—  cmx—      y—      z—  ^  ' 


oi  ^  =  0)  i+co  j+u)  k  (4.6) 
—  cmx—      y—      z—  V'/ 


and 


CO  =a)i+a)j+ojk  (4.7) 
—  cmx—      y—      z—  \  •  I 


Then  the  components  of  the  trans! ational  force  at  the  center  of 
mass  are  given  as 


F„  =  m(v  +  v  0)  -  V  co+r  w  -  r  co)  (4.8) 
X         ^  X       z    y       y    z       z    y       y    z'  \  "-^i 


F  =  m(v  +  v  (D  -  V  (D+r  u  -  r  u)  (4.9) 
y        ^y      xz      zx      xz      zx'  ^  ' 


F,  =  m(v,  +  v    CO    -  V    co+r    co    -  r    co)  (4.10) 
z         ^zyxxyyxxy'  ^  ' 


where 


V  =  (0       X  £  (4.11) 


The  moment  at  the  center  of  mass  ciue  to  a  rotation  of  the  body  by 

an  angular  velocity  co  can  be  expressed  in  the  principal  axes  frame  of 

the  object  by    Euler's  equations.    Let  I^^,  1^^,  I^^  denote  the  moments 

of  inertia  of  the  object  about  the  principal  axes.    If  M^,  M„  and  M,  are 

X      y  z 

the  three  components  of  the  moment  in  the  frame  of  the  principal  axes. 


128 

and  u' ,  to'  are  the  angular  velocity  and  the  angular  acceleration  of  the 
object  in  the  principal  axes  frame,  then  Euler's  equations  give 


M    =  I     oj'  +  (I    -  I    )  0)'  (o'  (4.12) 

X      XX    X        zz     yy'    y  X 


M    =  I     0)'  +  (I     -  I    )  oj'  0)'  (4.13) 

y       yy    y      ^  XX       zz'    z  y 


M    =  I     0)'  +  (I      -  I    )  u)'  0)'  (4.14) 
z      zz    z     ^  yy      zz'    X  z 


Thus  Eqns.  (4.8)  to  (4.10)  give  the  expressions  for  the  force  at 
the  object  center  of  mass  in  the  base  frame,  and  Eqns.  (4.12)  to  (4.14) 
the  equations  for  the  moment  in  the  frame  of  principal  axes  at  the 
center  of  mass  of  the  object. 

Factors  in  a  Force  Distribution  Scheme 

The  objective  of  the  force  distribution  scheme  is  to  generate  a 
load  distribution  taking  into  account  all  determinate  forces  on  the 
system.  The  various  factors  that  effect  load  distribution  are  surmised 
in  this  section.  Since  the  load  distribution  problem  is  underspecified, 
solution  techniques  for  solving  equations  that  are  fewer  in  number  than 
unknowns  must  be  employed.  If  all  equations  are  linear,  and  a  linear 
objective  function  is  formulated,  then  the  well  known  techniques  of 
Linear  Programming  can  be  used  to  solve  the  equations  [McGhee  and  Orin 
1976,  Orin  and  Oh  1981]. 

A  most  obvious  factor  for  minimization  in  the  load  distribution  for 
a  multi-manipulator  closed  chain  is  the  power  required  of  each 
manipulator  to  drive  its  end  effector  for  the  prescribed  load.  It  has 
been  shown  that  for  a  universal  series-wound  motor,  the    motor  power,  to 
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a  first  approximation,  is  directly  proportional  to  the  motor  torque 
[McGhee  and  Orin  1976],  Therefore,  the  minimization  of  joint  torques 
leads  to  a  reduction  in  motor  power.  The  joint  torques  for  a 
manipulator  with  no  external  force  at  its  end  effector  are  a  function  of 
the  robot  pose,  joint  rates  and  joint  accelerations.  A  simple  3-link 
planar  manipulator  (Figure  4.2)  is  used  to  show  the  dependence  of  the 
joint  torques  on  the  robot  pose.  The  plot  of  Figure  4.3  shows  the 
computed  torque  on  the  motor  of  joint  1  for  a  linear  motion  of  the  tip 
of  the  last  link.  The  2  plots  are  for  the  same  move,  except  that  the 
start-up  configuration  is  different  as  shown  in  Figure  4.3.  The  plots 
show  that  the  magnitude  of  the  torque  changes  significantly  for 
different  arm  configurations  along  the  same  Cartesian  move  of  the  end 
effector.  Thus,  it  is  likely  that  certain  manipulators  in  the  closed 
chain  may  be  in  a  configuration  that  yields  greater  forces  in  the 
required  direction  with  smaller  torque  requirements  than  other 
mani  pul ators . 

The  load  distribution  on  the  object  trajectory  takes  place  at 
specific  intervals  of  time,  determined  by  the  desired  sampling  rate  on 
the  trajectory.  Between  successive  object  poses  (position  and 
orientation),  some  effectors  translate  more  than  other  effectors.  Since 
the  trajectory  is  known,  the  work  done  by  each  effector  between  the 
current  and  the  succeeding  points  can  be  estimated  for  a  unit  force  in 
the  tangential  direction  of  the  end  effector  motion.  If  all  effectors 
are  to  share  the  load  equally,  then  one  of  the  factors  in  force  balance 
is  the  uniform  distribution  of  the  work  done  by  the  manipulators  between 
successive  sample  points.  In  other  words,  the  end  effector  translating 
more  than  another  end  effector  between  successive  sample  points  should 
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Figure  4.2 


3-link  planar  manipulator 
for  simulation 
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generally  carry  a  smaller  load.  In  order  to  determine  the  direction  of 
the  force  component  that  performs  useful  work  at  the  end  effector,  a 
coordinate  frame  called  the  "force  frame"  is  defined  from  the  space 
curve  of  the  end  effector  trajectory  for  each  effector.  The  force 
frames,  defined  in  the  next  section,  serve  as  body  attached  frames  at 
each  effector  for  the  determination  of  the  useful  component  of  the  end 
effector  force  (contributing  to  work).  The  force  frames  are  also 
defined  as  the  reference  frames  for  load  distribution. 

A  constraint  on  the  normal  tip  reaction  forces  must  ensure  that 
these  forces  are  well  within  the  specified  bounds  in  order  to  prevent 
the  deformation  of  the  object.  Other  factors  such  as  (1)  physical 
manipulator  limitations  on  torques  and  (2)  load  distribution  constraints 
described  in  the  secton  on  task  and  constraint  description,  also 
contribute  as  factors  in  force-moment  distribution. 

Definition  of  "Force"  Frames 

The  space  curve  trajectory  of  each  effector  indicates  the  direction 
of  the  useful  forces  acting  at  the  effector.  The  forces  performing 
useful  work  are  always  tangential  to  the  end  effector  space  curve.  The 
forces  in  the  normal  and  bi-normal  directions  of  the  effector  space 
curve  behave  like  constraint  forces  since  ideally  they  should  cancel. 
At  any  given  point  of  the  effector  trajectory,  we  can  construct  an 
orthonormal  coordinate  frame  with  the  z-axis  along  the  space  curve 
tangent,  x-axis  along  the  space  curve  normal  and  y-axis  along  the  bi- 
normal  direction  of  the  space  curve  trajectory.  This  moving  coordinate 
frame  is  called  the  force  frame. 


133 

We  employ  the  force  frames  to  express  the  force  and  the  moment  at 
each  effector  tip.  Since  in  load  distribution  we  seek  the  direction  and 
magnitude  of  the  force  and  moment  on  each  effector,  the  fixed  directions 
of  the  force  frame  can  be  used  to  express  the  load  distribution 
constraints  on  each  manipulator  for  a  unit  force  and  unit  moment  in  the 
3  directions  of  the  force  frame.  As  shown  later  in  the  chapter,  working 
with  the  manipulator  constraints  in  the  force  frame  simplifies  the  load 
distribution  problem,  as  compared  to  dealing  with  individual  constraints 
on  each  joint  of  the  manipulator. 

Derivation  of  the  Force  Frames  from  the  End  Effector  Trajectories 

Let  the  object  motion  be  described  by  the  following  6  polynomial 
functions  of  time: 

(1)  translation  along  x-axis  of  base  frame:  x(t) 

(2)  translation  along  y-axis  of  base  frame:  y(t) 

(3)  translation  along  z-axis  of  base  frame:  z(t) 

(4)  rotation  about  x-axis  of  base  frame:  ij;(t)  (yaw) 

(5)  rotation  about  y-axis  of  base  frame:    e(t)  (pitch) 

(6)  rotation  about  z-axis  of  base  frame:    (()(t)  (roll) 

The  six  polynomials  completely  specify  the  motion  of  the  object  by 
describing  the  translation  of  the  origin  of  principal  axes  (center  of 
mass)  and  the  rotation  of  the  principal  axes  frame  about  the  base 
frame.  The  choice  of  the  degree  of  the  polynomial  depends  upon  the 
desired  motion  characteristics.  Typically,  quartic  polynomials  provide 
continuity  in  acceleration  and  are  a  good  choice  for  representing  motion 
where  acceleration  continuity  is  desirable.  For  a  continuum  of  points, 
cubic  splined  polynomials  also  provide  continuity  in  acceleration. 
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Consider  the  instantaneous  motion  of  effector  "n",  attached  to  the 

object.     Let    r     be  the  radial  vector  from  the  center  of  mass  to  the 
—  n 

effector  n  (  £_  is  a  vector  in  the  base  frame).  The  tangent  to  the 
space  curve  of  effector  "n"  is  determined  from  two  velocity  components: 

•  •  • 

(1)  the  transl ational   velocity,  given  by    [x(t),  y(t),  z(t)]      ,  and 

(2)  the  rotational  velocity  component  at  the  center  of  mass,  given  by 
Ci(t),  e(t),  J(t)]  X  r  ^  . 


Let 


Ln  =  ^ni"  '2n   i  "  ^3n  ^  (^'^^^ 


Let   _v  j^i^  denote  the  rotational  velocity  component.  Then, 


V        [^(t).  0(t),  <|.(t)]  X  r  ^ 


^(^3n  ®-  ^2n      '  *  -;3n      '  ^ 

ir^,  I  -  r^,  e)] 


(4.16) 


If  |_  is  the  resultant  transl  ational  velocity  and  |^  is  the 
unit  vector  in  the  direction  of  v_  |^  (also  the  tangent  to  the  space 
curve  of  effector  "n"),  then 

Vl  I  L  =  t:^.       ^]  +  ^i^3n  ®  -  ^2n  ^^'/^In  *  ;  ^3n  ' 

.  ^'2n  \-  % 
=  ^(^^  ^3n         ^2n  ^  'in  \-  ^3n  '^]' 

(z+  r^^  i-  r^^  6)] 
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where 


The  normal  to  the  space  curve  of  effector  "n"  is  given  by  the  rate 
of  change  of  the  tangent  with  respect  to  the  space  curve.  The  direction 
of  normal  is  always  towards  the  center  of  curvature  of  the  space  curve. 


dTj_/ds  =  (dT|_/dt)  (dt/ds) 

=  (l/v^)  (dT^/dt)  =  <  N  L  (4.19) 


where  k  is  the  radius  of  curvature  (always  chosen  positive)  and  _N  ^  is 
the  normal  to  the  space  curve. 


<  N  L  =  (1/Vl)[(1/Vl)  {(x"  +  r3^  6"-  r^^  f)    ,    (y"  +  r^^  f  -  r^J') 

(z-.r^^r-r^^e-)} 


){(x+  r3^  0^-  r^^  ;).(;+  r^^  \  -  r^^  h, 
^2n  ^  -  ''in 


^"  -  ^in  -  ^^(^^  ^3n  'Zn 
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.  \  ^in  *  -  '^Sn  '2n     '  ^^^^ 

''in       -  ^3n        -  ^in  *  "  ^n  "  'Zn 

'2n        'lu       -  '2n        '^in  '^^^  ^^'^'^^ 

The  magnitude  of  the  vector  in  Eqn.  (4.20)  is  <  .  Eqns.  (4.17) 
and  (4.20)  give  the  vector  tangent  and  normal  to  the  space  curve  of 
effector  "n".  The  binormal  is  the  cross  product  of  the  tangent  and  the 
normal  vectors. 

The   direction   of   the   end   effector    space   curve   normal  becomes 

undefined  when  the  trajectory   is   linear.     Let  us  consider  a  linear 

motion  of  the  object  with  no  rotation.   In  this  case,  the  tangential 

vectors  to  the  space  curves  of  all  effectors  and  the  center  of  mass  are 

parallel.      The   direction   of   the   normal    (and   bi-normal)    vector  is 

arbitrary,  as  long  as  the  normal    is  perpendicular  to  the  tangential 

vector.    Since  the  tangential  vectors  of  the  effectors  and  the  center  of 

mass  space  curves  are  parallel,  we  propose  the  following  technique  to 

determine  a  common  normal   (and  bi-normal)  vector  for  the  linear  space 

curves.  The  technique  is  to  rotate  the  z-axis  of  the  principal  axes 

frame  into  the  tangential  vector  at  the  center  of  mass.  The  x  and  y-axes 

of  the   rotated  principal    axes   frame  yield  the  normal    and  bi-normal 

vectors  of  the  linear  space  curves, 
p 

Let  z_  denote  the  unit  z-vector  of  the  principal  axes  frame  (the 
superscript  P  shows  that  the  frame  of  reference  is  the  principal  axes 
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p 

frame).     If     t_     denotes  the  unit  tangential  vector  (along  the  linear 

p 

space  curve)  at  the  center  of  mass,  then  an  axis  of  rotation,  k^,  to 
rotate  the  z-axis  of  the  principal  axes  frame  into  the  tangential  vector 
is  obtained  as 


k  =  ^ z    X  ""t  L  (4.21) 


p 

The  angle  of  rotation  about  this  axis  is  the  angle  between     z_  and 

p 

t_  ^  .    Let    Y  denote  the  angle  of  rotation.  Then, 


cos  T  =    z    .  ^t^  (4.22) 


sin  Y  =  I  ""z    x  ''t  L  I  (4.23) 


Y  =  ATAN2  (  sin  y,  cos  y)  (4.24) 


where  ATAN2  is  the  inverse  tangent  function  that  returns  the  angle  and 
quadrant.  The  normal  and  bi -normal  vectors  to  the  linear  space  curve 
can  now  be  derived  from  the  rotation  matrix     R  as  follows: 


=  ^[Rp]  ^[Rot(^,  y)]  (4.25) 


where  l  Rp]  is  the  3x3  rotation  matrix  with  the  principal  axes  vectors 
expressed  in  the  base  frame,  and  Rot  (_k,  y)  is  the  rotation  matrix 
derived  in  [Paul  1981]. 
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Thus,  the  above  technique  can  be  employed  to  obtain  a  direction  for 
the  normal  and  bi -normal  vectors  when  the  space  curve  of  an  end  effector 
or  effectors,  or  the  center  of  mass,  or  both  the  effectors  and  the 
center  of  mass  (linear  motion  of  the  object)  is  linear. 

We  now  describe  two  algorithms  for  force-moment  distribution  in  the 
next  section. 

Algorithms  for  Force-Moment  Distribution 

In  this  section,  two  methods  are  described  for  force-moment 
distribution  in  a  closed-loop  multi -manipulator  system.  The  first 
method  simplifies  the  load  distribution  problem  by  decoupling  the  forces 
and  moments  between  successive  points  on  the  object  trajectory.  The 
decoupling  of  the  forces  and  moments  splits  the  load  distribution 
problem  into  2  independent  components.  Since  linear  programming 
techniques  are  employed  to  solve  the  underspeci f ied  problem  of  force  and 
moment  distribution,  the  decoupling  speeds  the  solution  since  the  number 
of  equations  and  the  initial  tableau  size  are  smaller.  The  decoupled 
load  distribution  problem  is  set  up  such  that  only  the  force 
distribution  requires  linear  programming.  The  moment  distribution 
follows  the  force  distribution  and  the  solution  for  moments  does  not 
require  linear  programming.  The  error  due  to  decoupling  of  the  object 
translational  and  rotational  forces  is  proportional  to  the  linear  and 
angular  velocities.  Thus,  the  decoupled  force-moment  distribution  is  a 
feasible  approach  for  slow  object  motions. 

If  the  error  due  to  the  decoupling  of  the  translational  and 
rotational  motions  is  not  tolerable,  then  a  combined  force-moment 
distribution  must  be  done  at  each  sample  point.    The  set-up  of  the  force 
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and  moment  equations  in  the  form  of  a  linear  programming  problem  is 
described  in  later  sections. 

For  any  linear  programming  problem,  an  objective  function  must  be 
defined.  The  solution  given  by  linear  programming  is  highly  dependent  on 
the  objective  function  defined  for  the  problem.  The  next  section  defines 
a  parameter  called  the  Average  Incremental  Work  for  each  manipulator 
(AIW)  that  is  employed  in  the  construction  of  the  objective  function. 

Definition  of  the  Average  Incremental  Work  per  Unit  Force  and  Moment 

In  this  section,  we  define  a  factor  called  the  Average  Incremental 
Work  (AIW)  for  the  manipulator  motors  when  a  unit  external  force  or  a 
moment  is  applied  at  the  end  effector.  The  AIW  provides  a  measure  of 
the  average  increase  in  the  motor  power  of  the  manipulator  to  counter  an 
external  force  or  moment  at  the  end  effector.  The  AIWs  are  computed  at 
each  "quasi-static"  [Vukobratovic  and  Stokic  1983]  sample  period  on  the 
object  trajectory.  Thus,  the  AIWs  are  determined  at  each  sample  instant 
where  a  load  distribution  is  required  on  the  object  trajectory.  Since 
the  reference  frames  for  the  load  distribution  on  the  object  are  the 
force  frames,  the  AIW  factors  are  defined  for  unit  forces  and  unit 
moments  in  each  of  the  3  directions  of  the  effector  force  frames  for 
each  arm.  Thus,  a  total  of  6  AIW  factors,  3  for  unit  forces  and  3  for 
unit  moments,  are  defined  for  each  manipulator.  The  AIWs  will  be 
determined  from  an  open-loop  dynamic  analysis  of  a  manipulator  chain. 
Initially,  the  incremental  joint  torques  from  no  load  to  a  unit  force  or 
moment  are  determined.  Then,  a  function  relating  the  motor  input  power 
to  torque  and  other  joint  parameters  is  used  for  determining  the 
incremental  power.  Since  a  criterion  for  load  distribution  is  to  reduce 
power,   the  AIW   factor   is   defined   as   the   average   incremental  power 
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required  of  each  motor  for  a  manipulator.  Note  that  the  AIWs  are 
nonlinear  functions  of  the  joint  torques.  This  nonlinearity  does  not 
affect  their  use  in  the  objective  function  of  the  linear  programming 
problem,  since  the  AIWs  will  be  constant  numbers  determined  apriori,  and 
the  objective  function  does  not  have  joint  torques  as  variables. 

Let  us  consider  a  manipulator  chain  whose  joints  are  powered  by 
series  wound  DC  motors.  To  a  first  approximation,  it  has  been  shown 
that  the  input  power  to  a  series  wound  DC  motor  is  directly  proportional 
to  the  joint  torque  [Orin  and  Oh  1981,  McGhee  and  Orin  1976].  The  AIW 
factors  for  this  case  are  defined  as  the  average  incremental  joint 
torques  for  a  unit  force  or  moment  in  the  3  directions  of  the  force 
frame.  Let  [t^,  tp,...,tg]  be  the  joint  torques  with  no  external  force 
or  moment  on  the  end  effector.  Let  [t^^p^^,  t2px»  •••  »  ^SFx-' 
joint  torques  for  a  unit  force  in  the  x-direction  of  the  end  effector 
force  frame.    Then  AIW^p^^  is  defined  as 

Note  that  the  joint  torques  [t^,  ...  ,tg]  and  [t^p^^,  ...  .tgp^]  can 
be  determined  by  an  "inverse"  dynamic  analysis.  The  data  required 
(other  than  the  parametric  data  for  the  manipulator)  are  external 
forces/moments  on  the  effector,  the  current  joint  positions,  velocities 
and  accelerations.  This  data  is  easily  derived  from  the  end  effector 
trajectory  [Whitney  1969,  Luh  et  al .  1980a].  The  procedure  for  the 
inverse  dynamic  analysis  can  be  either  one  of  2  well  known  approaches: 
Newton-Euler  [Luh  et  al .  1980a]  or  Lagrangian  [Hollerbach  1979]. 
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By  the  definition  of  the  AIW  factors,  the  higher  the  value  of  a 
force  or  moment  AIW,  the  smaller  should  be  the  respective  force  or 
moment  in  that  direction  of  the  force  frame.  An  additional  work  factor, 
described  in  the  section  on  factors  in  a  force  distribution  scheme,  is 
added  to  the  force  AIWs  in  the  tangential  direction  of  the  end  effector 
space  curves.  This  factor  is  the  work  done  by  each  effector  between 
successive  sample  points  on  its  trajectory.  Since  the  load  distribution 
should  provide  a  uniform  distribution  of  the  load  between  sample  points, 
the  work  done  by  each  effector  between  sample  points  given  by  the  line 
integral  of  the  tangential  force,  is  added  to  the  force  AIW  factor  in 
that  direction.  Since  the  z-axis  of  the  force  frame  denotes  the 
tangential  direction,  the  AIW^p^  is  modified  as 

AIW.p^  =  k^    AIW.p^  +    k^l  /f        dz  I  (4.27) 

where  k^  and  k2  are  weighting  factors. 

The  integral  in  Eqn.  (4.27)  is  computed  for  a  unit  force  along  the 
tangent  _f  from  the  knowledge  of  the  end  effector  space  curve 
between  the  sample  points. 

A  Decoupled  Force-Moment  Balance 

In  order  to  simplify  force-moment  distribution,  the  translation  and 
rotation  of  the  object  can  be  decoupled  for  each  incremental  motion  of 
the  system.  The  decoupled  motion  of  the  object  center  of  mass  becomes  a 
pure  translation  followed  by  a  rotation  about  an  axis.  During 
translation,  all  effector  trajectories  will  be  parallel  to  the  center  of 
mass  trajectory.      Therefore,   the   force   frames   of  all    effectors  are 
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parallel  during  the  translation  phase  of  the  motion.  The  force  at  the 
center  of  mass  is  simply  the  sum  of  the  gravitational  force  (m_£_)  and  the 
linear  translational  force  (m^) . 

£   =  m  (a_+  £)  (4.28) 

The  effect  of  the  angular  velocity  and  angular  acceleration  on  the 
forces  at  the  effectors  is  ignored.  The  error  "e"  in  the  translational 
force  on  an  effector  due  to  ignoring  the  angular  velocity  effect  can  be 
derived  as 


e  =  m.  (oj  ^„  X  (ti)  X  r)  +  oj  x  r)  (4.29) 
—  —  cm      —  cm     —      —  cm  — 


where    oo     ,  oo        are  the  angular  velocity  and  angular  acceleration, 
—  cm   —  cm 

and  r_  is  position  vector  from  the  origin  of  the  base  frame  to  the 
center  of  mass.  If  the  rotational  velocity  and  rotational  acceleration 
are  small,  then  £  given  by  Eqn.  (4.29)  can  be  ignored,  and  the 
simplified  load  distribution  of  this  section  applies. 

.The  force  at  the  center  of  mass  in  base  coordinates,  given  by  Eqn. 
(4.28)  can  be  resolved  into  3  components  along  the  vectors  of  the  force 
frame  at  the  center  of  mass.  Let  {^q^^,  ^cmy  '^cmz^  denote  the  3 
components  of  the  center  of  mass  translational  force  along  the  axes  of 
the  force  frame. 


('^cmx'  ^my  f^cmz)'  =  '^^B  ^'  (^'^O) 
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where  '^Rg  is  a  3x3  rotation  matrix  from  the  base  frame  to  the  center  of 
mass  force  frame  (see  [Paul  1981]  for  the  derivation  of  static  force 
transformations  between  coordinate  frames). 

Let  (f^^,  fjy,  f-^^),  (f2x,  ■f2y'  ^2z'>'  "'  '^^nx'  '''ny'  '•"nz^ 
represent  the  forces  transformed  from  the  base  frame  to  the  force  frames 
of  the  n  end  effectors.  Since  all  force  frames  are  parallel  for  a  pure 
translation,  the  following  3  force  equality  equations  are  true. 


^Ix  ^  ^2x  "'  ^  ^nx  ''cmx 


(4.31) 


f,    +  fo   +  ...  +  f      =  F 

ly       2y  ny  cmy 


(4.32) 


f,    +  fo   +  ...  +  f      =  F 

Iz       2z  nz  cmz 


(4.33) 


Note  that  we  have  3  equations  and  3  *  n  unknowns.  Therefore,  for  n 
>  1,  there  is  no  unique  solution  to  the  forces  on  the  end  effectors. 
Since  forces  and  moments  are  decoupled  in  this  section,  we  initially 
address  force  distribution  and  then  moment  distribution.  The  force  and 
moment  distribution  equations  will  be  set  up  as  two  separate  problems. 

In  order  to  decouple  force  and  moment  distribution,  we  impose  the 
constraint  on  the  forces  at  the  end  effectors  that  their  resultant 
moment  at  the  center  of  mass  is  zero.  Thus,  if  r_  |^  denotes  the  vector 
from  the  origin  of  the  force  frame  at  the  center  of  mass  to  the  effector 
k  (  _r  .  referenced  to  the  force  frame  at  the  center  of  mass),  then 


(4.34) 
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where    ^"'Rj^  is  a  3x3  rotation  matrix  that  transforms  a  force  in  the 

force  frame  of  effector  k  to  the  corresponding  force  in  the  frame  at  the 

center  of  mass.     Since  the  force  frames  for  the  load  distribution  of 

cm 

this  section  are  all  parallel,  the  rotation  matrix  Rj^  is  an  identity 
matrix. 

During  force  distribution,  other  than  minimizing  the  power  input, 
inequality  constraints  on  the  maximum  normal  force  on  the  object  must  be 
introduced  to  prevent  deformation  of  the  object.  Also,  the  magnitude  of 
the  joint  torques  must  be  checked  against  a  maximum  specified  value  for 
the  motor.  The  constraints  on  the  joint  torque  are  ignored  in  this 
section  because  (1)  the  system  angular  velocity  and  angular  acceleration 
are  low,  and  (2)  force  distribution  according  to  the  AIW  factors  leads 
the  system  towards  smaller  torques.  The  constraints  on  the  forces 
normal  to  the  object  at  the  end  effector  contact  points,  however,  cannot 
be  ignored.    Let  f^-jmax  maximum  specified  magnitude  of  the  object 

surface  normal  force  for  effector  "i".  Let  fj^^  be  the  magnitude  of  the 
surface  normal  force  due  to  the  force  distribution. 

Then, 

I^Ni  I  '  'mm. 

The  magnitude  function  in  Eqn.  (4.35)  is  a  non-linear  operator,  and 
cannot  be  used  in  the  equations  for  linear  programming.  In  order  to 
"linearize"  Eqn.  (4.35),  an  additional  2  variables  are  added  [Llewellyn 
1969,  Orin  1979]  such  that 

^Ni  =  4  - (^-36) 
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and 


^'m-  +  ^Kr   <  (4.37) 


The  variables  f^^  and  fj^^^  are  both  positive.  The  inequality 
(4.37)  can  be  transformed  to  a  linear  equation  by  the  addition  of  a 
"slack"  variable  x^-  as  follows 


f,,.  +  f,,.  +  x.  =  f...  (4.38) 
Ni       Ni       1       Nimax  ' 


Thus  2*n  additional  equations  (Eqns.  (4.36),  (4.38))  are  added  to 
the  force  distribution  problem. 

The  objective  function  is  formulated  such  that  the  overall  system 
power  is  reduced.  The  AIW^  ,  defined  in  the  preceding  section,  estimates 
the  relative  increase  in  the  power  supplied  to  manipulator  "i"  in 
response  to  an  external  force.  If  the  AIW  is  large  for  a  manipulator, 
then  the  load  distribution  should  seek  other  manipulators  that  require 
less  power,  but  allow  the  creation  of  the  desired  motion.  Thus,  the 
external  force  at  an  end  effector  of  a  manipulator  should  be  inversely 
proportional  to  the  respective  AIW  factor.    We  define  a  set  of  factors 

AAA 

(o.  ,  a.  ,  a.  )  for  i  =  1  to  n  as  follows: 
^  ix'    iy'  m' 

A 

(1/AIW.^)  (4.39) 
(1/AIW.y)  (4.40) 

A 

(1/AIW.^)  (4.41) 


a.  = 

IX 

A 

a.  = 

A 

a.  = 
1  z 
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For  reduction  in  power,  the  forces  should  be  directly  proportional 

A  A 

to  the  set  of    (a.  ,  a.  ,  a.  ) .  At  this  stage,  we  introduce  user  defined 
1 X     1  y     1  z  A 

constraints  on  the  manipulator  payloads  (in  Kilograms)  to  modify  "a"  as 
follows.  Let  the  payload  ratio  of  the  n  manipulators  in  the  chain  be 
given  as 


1^:  l^:  I3  ...  :  1^  (4.42) 


Each  set  of    (a.  ,  a.  ,  a.  )  is  modified  according  to  the  weighted 
IX     1  y     1  z 

average  of  the  payload  ratios.    Thus,  for  effector  "i", 

n        -       -  - 

.)  (4.43) 


(a.  ,  a.  ,  a.  )  =  (1./  El.)(a.  ,  a.  ,  a. 
^  ix'    iy'    m'      ^  1     ._.  j'^  ix'    iy'  1 


Z' 


Thus  Eqn.  (4.43)  modifies  the  a  -factors  such  that  each  set  of 
a  -factors  for  a  manipulator  reflect  the  pro-rated  share  of  the 
manipulator's  payload. 

Without  the  force  constraints  of  this  section  (  (1)  constraint  on 
the  magnitude  of  the  effector  surface  normal  forces,  and  (2)  constraint 
on  the  effector  forces  that  the  moment  at  the  center  of  mass  is  zero), 
the  force  at  each  effector  should  equal  a  proportion  of  the  force  at  the 
center  of  mass  determined  by  a  ratio  of  the    a  -factors  as  follows: 

f|z=  W  «iz/  °jz)  (^-44) 


where    fl^  is  the  force  in  the  z-di  recti  on  of  the  effector  force  frame 

expressed  in  the  center  of  mass  force  frame  and    F       is  the  force  in 

cmz 

the   z-direction   of  the   center  of  mass   force   frame.   For  the  linear 
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programming  solution,  we  formulate  the  objective  function  so  as  to  pair- 
wise  cancel  the  end  effector  force  and  the  corresponding  force  obtained 
by  the  distribution  of  the  center  of  mass  force  strictly  in  accordance 
with  the  a  -factors.  If  f^-^  denotes  the  z-force  component  of  the  ith 
effector,  then  a  factor  A-j^       defined  as  follows: 

Z  a. 
j=l 

The  objective  is  to  minimize  A^^  ,  because  when  A^^  =  0,  the 
force  at  the  effector  is  exactly  in  the  desired  ratio  of  the  a's.  The 
objective  function  is  therefore  defined  as 

MINIMIZE  [  _Z^[|  A.J  +   I  A.^1  +   I  A.J)]  (4.46) 

Thus,  the  linear  programming  set-up  for  force  distribution  has  (2n 
+  3)  equations  with  6*n  unknowns.  The  objective  function  of  Eqn.  (4.46) 
tends  to  reduce  the  input  power  and  also  attempts  to  distribute  the 
forces  according  to  the  specified  pay  load  capabilites  of  the 
manipulators. 

Distribution  of  Moments 

The  moment  at  the  center  of  mass  is  due  the  rotation  of  the  object 
about  an  axis  in  space.  Let  M^.^^^,  M^.^^,  M^.^^^  denote  the  3  components  of 
the  moment  at  the  center  of  mass  referred  to  the  force  frame.  The 
equations  for  the  moment  components  in  the  force  frame  are 
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cmx 


cmy 


cmz 


Fcnir 


I  0)'     +  (I 
XX      X      ^  zz 

I  li'    +  {I 
yy    y  xx 

I  0)'     +  (I 

zz     z      ^  yy 


I       )    Co'  0)' 

yy     y  z 

I      )    0)'  UJ' 

zz       x  z 


I      )      0)'  0)' 

XX '      x  y 


(4.47) 


where  Rp  is  the  3x3  transformation  matrix  from  the  principal  axes 
frame  to  the  center  of  mass  force  frame  and  w' ,  u'  are  the  angular 
velocity  and  acceleration  of  the  object  in  the  principal  axes  frame. 

Let    (m^x.   m^^y,   "^Iz^'    ^^2x>   ^2y'   "^2z)»    •••    ♦    ('"nx*   %y>  "^nz^ 
represent    the    moments    in    the    force    frame    directions    of    the  n 

effectors.    Since  all  force  frames  are  parallel. 


m,    +        +  . . .  +  m     =  M 

Ix       2x  nx  cmx 


(4.48) 


m,    +        +  . . .  +  m     =  M 

ly       2y  ny  cmy 


(4.49) 


m,    +  m^  +  . . .  +  m     =  M 

Iz       2z  nz  cmz 


(4.50) 


The  moment  distribution  from  Eqns.  (4.48)  -  (4.50)  is  similar  in 
principle  to  the  force  distribution.  The  moments  in  the  effector  force 
frames  should  be  inversely  proportional  to  the  AIW  factors  determined 
for  unit  moments  in  each  direction  of  the  force  frame.  We  define  a  set 
of  factors  (B^-j^,  e^y,  3^^)  ^°  "  follows: 


e.    =  1/(AIW)  . 
IX        ^  'mix 


(4.51) 


e,  =  i/(Aiw)  . 

iy        ^  'miy 


(4.52) 
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6.    =  1/(AIW)  . 


(4.53) 


Since  no  further  constraints  are  imposed  on  moment  distribution, 
the  solution  to  Eqns.  (4.48)  -  (4.50)  can  be  efficiently  determined  by  a 
direct  weighted  average  ratio  of  the  factors  given  by  Eqns.  (4.51)  - 
(4.53).  Thus, 


m. 


n 

2  6, 


cmx 


j  =  l 


JX 


m. 


iy 


n 
j  =  l 


cmy 


jy 


m.    =    M 

iz        n  cmz 


^  3, 


j  =  l 


JZ 


(4.54) 


(4.55) 


(4.56) 


A  General  Force-Moment  Distribution  Scheme 

The  decoupled  force-moment  distribution  scheme  of  the  previous 
section  provided  an  approximate  force  moment  balance  for  slow  motions  of 
the  closed-loop  system.  In  this  section,  we  address  the  general  force- 
moment  distribution  with  inequality  constraints  on  the  maximum  normal 
force  and  the  maximum  joint  torques  of  each  manipulator.  As  stated 
before,  the  frictional  forces  are  ignored. 

This  section  is  organized  as  follows.  Initially,  we  derive  the 
equality  equations  between  the  center  of  mass  force  and  moment  and  the 
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end  effectors  forces  and  moments.  The  inequality  constraints  on  the 
maximum  normal  force  and  the  maximum  joint  torques  are  described  next. 
Finally,  the  objective  function  for  the  solution  of  the  linear  equations 
is  derived. 

Force  and  Moment  Equality  Equations 

The  force  components  at  the  center  of  mass  in  the  base  frame  are 
(Eqns.  (4.8)-(4.10)) 


F  =m.  (v  +  v  oj  -  V  oj+r  w  -  r  a))'  (4.57) 
X  ^xzyyzzyyz' 


F    =m.  (v   +  v    0)   -  V    (D+r    u-r    u)  (4.58) 

y  ^yxzzxxzzx'  ^  ' 


F    =m.  (v+v    0)    -  V    to+r    u    -  r    u)  (4.59) 

z  wyxxyyxxy'  ^  ' 

ys  A  A 

where     (v  ,  v  ,  v  )  are  defined  by  Eqn.   (4.11),     (co  ,  o)  ,  u)  )  are  the 
X     y     z  X     y  z 

•      •  • 

components  of  the  angular  velocity  and    (v  ,  v  ,  v  )  are  the  components 

X     y  z 

of  the  linear  acceleration,  all  vectors  referenced  to  the  base  frame. 

If    f_  .  ,  f_  .  ,  jf  ^-    denote  the  forces  in  the  3  directions  of  the  force 
IX       1 y       1  z 

frame  of  effector  i,  then  the  3  equality  equations  for  the  forces  can  be 
derived  from  the  following  vector  equation: 


n 

F       =    I  (f  .  +  f  ,  +  f  .  )  (4.60) 

—  cm  1    —  IX   —  iy   —  m'  ' 


where  ^"^R^-  is  a  3  X  3  rotation  matrix  that  transforms  a  vector  in  the 
force  frame  of  effector  "i"  to  the  center  of  mass  force  frame. 

The  moment  equations  relating  the  moments  at  the  center  of  mass  to 
the  moments  on  the  effectors  is  derived  as 
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(4.61) 


(4.62) 


z-comp 


(4.63) 


where     (M  ,  M  ,  M  )  denotes  the  moment  due  to  the  object  rotation  and 
\  x'   y'  z' 

this  moment  in  the  force  frame  at  the  center  of  mass  is  given  by  Eqn. 
(4.47).  The  moment  equality  equations  are  now  given  by  the  following 
vector  equation: 


Inequality  Constraints  on  Normal  Forces  and  Joint  Torques 

The  inequality  constraints  on  the  normal  forces  at  the  n  effectors 
are  expressed  as  Eqns.  (4.35)  and  (4.37).  The  conversion  of  the 
inequality  constraints  to  linear  equations,  as  in  Eqn.  (4.37), 
introduces  a  total  of  n  "slack"  variables  in  the  linear  programming 
probl em. 

Since  there   is   an   upper  and  lower  bound  on  the  joint  torques, 

inequality  constraints  on  the  magnitude  of  the  torques  must  also  be 

included.    In  order  to  check  for  a  torque  bound,  given  an  external  force 

at  tne  effector,  we  employ  the  values  of  the  open-loop  torques  per  unit 

force  and  moment  which  are  derived  for  the  determination  of  the  AIWs. 

'-^^     ('^ivi*  "^-iwi'  ""-iT-i)  denote  the  open  loop  torques  expressed  in  the 
'  xj      lyj      1  zj 

base  coordinates,  of  joint  j  per  unit  force  and  unit  moment  in  the  x,  y 


n 
Z 

i=l 


cm 


.  (m  .  +  m  .  +  m  .  ) 
1    —  IX     —  iy     —  ^z' 


(4.64) 
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and  z  directions  of  the  force  frame  of  effector  i.  Thus,  for  a  given 
force  and  torque  at  an  effector,  an  estimate  of  the  torque  requirement 
from  the  joints  is  obtained  by  the  linear  relationship  between  the 
forces  and  moments  at  the  end  effectors  and  the  joint  torques. 


(t.)    ^  =  (a.    f .  )  T.   .  +  (a.    f .  )  T.   .  +  [a.    f.  )  t.  . 

^  j'est     ^  IX    ix'  1XJ      ^  iy    iy'    lyj      ^  iz    iz'  lyj 

+  (u.    m.  )  T  .   .  +  (y.    m.  )  T  .   .  +  (y.    m.  )  t  .   .  cc\ 

^  IX    ix'  mixj      ^  iy    ly'    miyj      ^  iz    iz'    mizj  (4.65) 


where  (a.^,  a.^,  a.^)  and  (y.^,  y.^,  y.^)  are  porportional ity 
constants . 


I  (t.)  <  (x.)  (4.66) 
'  ^  J 'est'       ^  J 'max  ^  ' 

The  transformation  of  Eqn.   (4.66)   into  linear  equations   (as  for 
in  Eqns.  (4.36),  (4.38))  introduces  an  additional  2*n*m  equations, 
where  n  is  the  number  of  manipulators  and  m  is  the  number  of  joints  per 
manipulator. 

Formulation  of  the  Objective  Function 

The  formulation  of  the  objective  function  is  identical  in  principle 
to  the  objective  function  of  the  previous  section.  The  idea  is  to 
determine  a  set  of  values  for  the  effector  forces  and  moments  such  that 
these  values  are  as  close  as  possible  to  the  ratios  of  the  as  and  3s 
defined  by  Eqns.  (4.43)  and  Eqns.  (4.51)  to  (4.53).  Denoting  the 
effector  forces  as  (f^^,  f^^,  f^^)  •♦•  (^nx*  ^ny»  ^nz^'  effector 
moments  as  (m^^,  m^^,  m^^)  ...  (m^^,  m^^,  m^^)*  ^ifx  ^imx 
defined  as 
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A.  . 
ifx 


=  [ 


A. 
imx 


f . 

1  X 

"ix 

n 

n 

E    f . 
j=l  J' 

Z  a. 
j  =  l 

m. 

1  X 

^•x  ^ 

n 

n  ' 

E  m. 
j=l 

Z  g., 

(4.67) 


(4.68) 


Similarly,  A^-^^,  A^^^,  A^-^^  and  A^-^^^  can  be  defined.  The  objective 
function  is  therefore 


n 


MINIMIZE  [  /^(lA.^J  +  lA.^^I    +  |A.^^ 


i  =  l  "J'  (4.69) 

n 

+     E     (  I  A.      I  +   A.      I  +   A.      I )] 
^'  imx'       imy'  imz" 


Thus,  the  general  force-moment  distribution  has  a  total  of  [2n(m<-l) 
+  6]  equations,  where  n  is  the  number  of  manipulators  and  m  the  number 
of  links  per  manipulator,  (6  force-moment  equality  equations,  2n 
equations  for  the  normal  force  limit  inequality,  2nm  equations  for  the 
maximum  torque  inequality)  with  [3n(m  +  1)  +  6n]  variables. 

An  Illustrative  Example 

In  this  section,  we  describe  the  simulation  results  of  the  load 
distribution  schemes  of  the  preceding  section  on  a  simple  2  manipulator 
closed-loop  task.  The  manipulators  are  3-link  3-degrees  of  freedom 
planar  manipulators  (Figure  4.2).  Each  link  of  the  planar  manipulator 
is  assumed  2  m  long.     All  links  are  cylindrical  with  masses  of  1  Kg. 
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each.  The  task  set  up  is  shown  in  Figure  4.4.  F-^  and  F2  denote  the 
"base"  frames  of  the  2  manipulators.  The  object  is  a  uniform  rigid  cube 
with  1  m  side  length.  F^^^^  denotes  the  object  frame  at  the  center  of 
mass.  The  objective  of  the  simulated  task  is  to  translate  and  rotate 
the  object  from  to  P3  (Figure  4.4).  A  constant  translational 
acceleration  of  0.25  m/sec^  is  assumed.  It  is  also  assumed  that  the 
start  up  velocity  of  the  object  at  P^^  is  1  m/sec  and  the  destination 
velocity  at  P3  (along  P2-P3)  is  also  1  m/sec.  Thus,  the  task  is  to 
transform  the  object  at  P-^  with  a  linear  velocity  of  1  m/sec.  (in  the 
direction  P^^  P2)  to  P3  with  a  uniform  acceleration  of  0.25  m/sec'^,  and 
rotate  the  object  about  the  z_  axis  a  total  of  ((>  =  n/3  radians  between 
P^  and  P3.  The  move  time  for  the  segment  P1-P3  is  calculated  [Taylor 
1979]  as 


2t  =  I  V  „  -  v  J  /  a„^^  =  K  3/4  +  1/4  I  /  0.25  =  2  sees.  (4.70) 

The  object  is  rotated  II/3  radians  between  P-^  and  P3.  The 
rotational  velocity  (about  the  z-axis)  is 

0)^  =  <j>/2T  =  n/6    rad/sec  (4.71) 

The  center  of  mass  trajectory  between  P^^  and  P3  can  be  expressed  as 
a  function  of  time  as 


0    <  t  <  2t 


(4.72) 


Figure  4.4 


Two  manipulator  task  for  simulation 
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where 

P_  ^  is  the  start  point, 

=  linear  velocity  (1  m/sec) 
^  =  unit  vector  in  the  direction       -  P2 

a     =  (1  2  "  l^/^"" 

The  simulation  program  for  testing  the  load  distribution  schemes  of 
this  chapter  comprises  the  following  5  Pascal  program  modules:  (1) 
kinematics  of  the  3-link  planar  manipulator,  the  forward  and  reverse 
solutions,  (2)  dynamics  of  the  3-link  planar  manipulator,  routines  for 
reverse  velocity  and  acceleration  and  a  routine  for  determination  of  the 
joint  torques,  (3)  a  module  for  path  generation,  (4)  a  module  for 
solving  linear  equations  employing  the  Simplex  method  [Llewellyn  1969], 
and  (5)  a  module  for  the  determination  of  the  load  distribution  factors 
and  setting  up  the  load  distribution  equations.  The  object  trajectory 
is  sampled  every  50  ms.  At  each  sample  period,  the  joint  torques  are 
computed  from  the  trajectory  data  and  the  load  distribution  at  the  end 
effectors.  For  simplicity,  no  constraints  are  imposed  on  the  maximum 
joint  torques  and  the  end  effector  normal  forces. 

The  plots  of  Figures  4.5,  4.6  and  4.7  show  the  joint  torques  versus 
time  for  one  of  the  manipulators  in  the  closed-loop  task.  In  order  to 
illustrate  the  reduction  in  the  overall  system  torque  (power),  we  plot 
the  sum  of  the  magnitudes  of  the  joint  torques  of  both  manipulators  for 
a  load  distribution  employing  the  a  -factors  of  Eqns.  (4.39)-(4.41) , 
and  a  load  distribution  by  assuming  equal  loads  at  both  end  effectors 
for  all  configurations.  The  plots  are  shown  in  Figure  4.8.  We  notice 
that  the  load  distribution  for  this  example  reduces  the  overall  system 
torque  over  that  of  an  equal  load  distribution.    A  plot  of  one  of  the 
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AIW  factors,  namely  the  force  AIW  factor  in  the  x-direction  of  the  force 
frame  of  each  effector  in  Figure  4.9  shows  the  change  in  AIW^  versus 
time  for  the  simulated  move. 
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CHAPTER  V 
CONCLUSION 

This  dissertation  provides  new  solutions  for  on-line  tracking  of 
end  effector  trajectory,  "feasible"  and  accurate  motion  planning  by 
creation  of  a  "rotation  space"  and  independent  deterini  nation  of 
rotational  motions,  and  force  coordination  in  a  multi-robot  task.  The 
main  contributions  are  surmised  as  follows: 

(1)  A  new  model  is  developed  for  joint  interpolated  motion.  The 
model  employs  cubic  polynomials  for  start-up  and  set-down.  A  novel 
feature  of  the  model  is  the  constraint  on  coordination  of  the  pacing  and 
non-pacing  joints  during  start-up,  cruise  and  set-down.  This  constraint 
reduces  the  start-up  jerk  in  the  non-pacing  joints  by  clipping  the 
acceleration  during  the  start-up  and  set-down  without  affecting  the 
total  move  time.  The  performance  evaluation  of  the  joint  motion  model 
by  simulation  and  experimentation  on  an  industrial  robot  demonstrates 
the  superiority  of  the  proposed  model  to  the  current  model. 

(2)  We  developed  an  on-line  trajectory  approximation  technique  for 
the  joint  trajectories  during  a  constrained  end  effector  motion.  We 
derive  a  factor  for  the  determination  of  the  ideal  number  of  look-ahead 
points  on  the  end  effector  trajectory.  This  factor,  called  the  system 
memory  error,  is  indicative  of  a  certain  level  of  performance  in  the 
trajectory.  The  primary  attributes  of  the  on-line  cubic  splines  are 
tracking  accuracy  and  continuity  in  at  least  the  first  derivative.  The 
testing    of    the    on-line    cubic    splines    by    simulation    and  actual 
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experimentation  on  an  industrial  robot  showed  excellent  results  in  terms 
of  tracking  accuracy  and  smooth  motion. 

(3)  We  define  and  develop  independent  rotational  planning  of  end 
effector  motions.  A  "rotation  space"  is  defined  for  spherical  wrist 
manipulators  by  the  manipulator  degrees  of  freedom  from  the  wrist  to  the 
effector.  We  proved  that  the  configuration-free  estimate  of  the  maximum 
tool  rotational  acceleration  in  rotation  space  is  the  full -load 
acceleration  of  the  slowest  joint  in  the  rotation  space. 

(4)  A  method  for  independent  rotational  planning  of  both 
unconstrained  and  constrained  tool  rotations  is  derived.  We  describe  a 
technique  for  unconstrained  point-to-point  rotational  trajectories 
employing  the  maximum  constraints  on  the  joint  accelerations.  A 
simulation  example  where  the  existent  translation-dominant  techniques 
fail  to  produce  a  "feasible"  motion  clearly  illustrates  the 
applicability  of  independent  rotational  planning  for  producing  smooth 
and  feasible  motions. 

(5)  Two  linear  models  are  developed  for  the  active  distribution  of 
the  object  load  among  the  manipulators  in  the  coordination  of  a  closed 
chain  multi-manipulator  task.  A  set  of  moving  coordinate  frames,  called 
force  frames,  are  defined  at  each  effector  and  the  object  center  of  mass 
as  reference  frames  for  load  distribution.  It  is  shown  that  the  force 
frames  simplify  the  formulation  of  constraints  on  the  load  distribution 
problem.  A  linear  model  for  load  distribution  is  formulated  such  that 
(a)  the  system  power  is  reduced,  (b)  the  joint  torques  are  limited  to 
their  respective  maximum  values,  (c)  the  magnitude  of  each  surface 
normal  force  on  the  object  is  constrained  to  a  maximum  value,  and  (d) 
the    load    distribution    reflects    the    payload    capabi 1 itites    of  the 
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individual  manipulators.  A  simulation  of  the  proposed  load  distribution 
model  demonstrates  the  reduction  in  the  system  power  at  each  sample 
instant. 

The  work  in  this  dissertation  is  one  step  towards  greater 
cooperation  in  multi-robot  tasks.  Most  of  the  results  presented  in  this 
dissertation  have  been  verified  either  by  simulation  or  experimentation, 
or  both.  The  linear  models  of  Chapter  IV  for  the  load  distribution  in  a 
closed  chain  can  be  refined  to  include  (1)  prediction  and  compensation 
of  the  interactive  forces  and  moments  in  the  load  distribution,  (2) 
inclusion  of  frictional  forces,  and  (3)  allowing  degrees  of  freedom 
between  the  object  and  the  effectors. 
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